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 / epipolar.lisp < prev    next >
Lisp/Scheme  |  1995-07-20  |  73KB  |  1,624 lines
  1. ;;; -*- Mode:Common-Lisp; Package:USER; Fonts:(MEDFNT HL12B HL12BI MEDFNT MEDFNB); Base:10 -*-
  2.  
  3. (in-package 'user)
  4.  
  5. ;;;;FIRST MUST EXECUTE (load "unix:lisp;load-new-vanishing-point")
  6. ;;;;  (load "unix:lisp;histogram")
  7. ;;;;  (load "unix:lisp;plot")
  8.  
  9. (defvar *demo-mode* t "tells whether to display things while processing")
  10.  
  11. (defvar *lowz* -2.2)
  12. (defvar *highz* 3.5)
  13. (defvar *1overcamz* (/ 1.0 165.33823))
  14. (defvar *num-z-buckets* 24 "default number of histogram buckets")
  15. (defvar *current-histogram* nil "height histogram for this image")
  16. (defvar *accum-histogram* nil "accumulated height histogram")
  17. (defvar *spline-xarr* nil "array for display histograms as splines")
  18. (defvar *spline-yarr* nil "array for display histograms as splines")
  19.  
  20. (defvar *display1* nil "display window")
  21. (defvar *display2* nil "second display window")
  22. (defvar *plot1* nil "plot window")
  23. (defvar *plot2* nil "second plot window")
  24. (defvar *no-labels* nil "if nonnil, means don't display window labels - for video purposes")
  25.  
  26. ;;;;====================  COLOR AND THICKNESS SETTINGS ========================
  27.  
  28. (defvar *histogram-color* w:black)
  29. (defvar *histogram-thickness* 3)
  30. (defvar *epipolar-color* w:green)
  31. (defvar *epipolar-thickness* 3)
  32. (defvar *search-color* w:blue)
  33. (defvar *search-thickness* 3)
  34. (defvar *line-color* w:black)
  35. (defvar *line-thickness* 1)
  36.  
  37. ;;;============================================================================
  38. (defvar *j1grid* nil)
  39. (defvar *j2grid* nil)
  40. (defvar *j3grid* nil)
  41. (defvar *j4grid* nil)
  42. (defvar *j5grid* nil)
  43. (defvar *j6grid* nil)
  44. (defvar *j7grid* nil)
  45. (defvar *j8grid* nil)
  46.  
  47. (defvar *j1numrows* nil)
  48. (defvar *j2numrows* nil)
  49. (defvar *j3numrows* nil)
  50. (defvar *j4numrows* nil)
  51. (defvar *j5numrows* nil)
  52. (defvar *j6numrows* nil)
  53. (defvar *j7numrows* nil)
  54. (defvar *j8numrows* nil)
  55.  
  56. (defvar *j1dlt* (make-array '(3 4) :initial-contents '(
  57.    (-19.960773   0.22749877   -12.903301   842.667 )
  58.    (-3.4838574   -23.273052   5.303175   1114.2806 )
  59.    (0.0015737481   -0.0012492101   -0.0026543885   1.0))))
  60.  
  61. (defvar *j2dlt* (make-array '(3 4) :initial-contents '(
  62.    (-1.7695084   -19.28334   13.439316   1076.7924 )
  63.    (23.069756   0.33550453   3.4454346   188.71094 )
  64.    (0.000654608   -0.002153894   -0.0017601326   1.0))))
  65.  
  66. (defvar *j3dlt* (make-array '(3 4) :initial-contents '(
  67.    (-23.920588   -1.4487858   -1.3064575   933.0063 )
  68.    (1.5003289   -24.09358   -0.61531067   976.0755 )
  69.    (-0.0000140998745   -0.0002232939   -0.0027317852   1.0))))
  70.  
  71. (defvar *j4dlt* (make-array '(3 4) :initial-contents '(
  72.    (0.4836445   19.066517   -13.104324   188.70502 )
  73.    (-23.331348   -0.13543749   -1.3505707   812.1729 )
  74.    (-0.00026167184   -0.0017117839   -0.003778996   1.0))))
  75.  
  76. (defvar *j5dlt* (make-array '(3 4) :initial-contents '(
  77.    (-0.5118285   23.024166   9.621094   314.9109 )
  78.    (-24.943178   0.011466533   -0.47771835   763.81146 )
  79.    (-0.000101438665   0.001958397   -0.0029221326   1.0))))
  80.  
  81. (defvar *j6dlt* (make-array '(3 4) :initial-contents '(
  82.    (2.3561907   23.421192   -5.657258   145.03972 )
  83.    (-23.42959   0.923416   -5.657257   766.7126 )
  84.    (0.0005539898   -0.00046683103   -0.0041301455   1.0))))
  85.  
  86. (defvar *j7dlt* (make-array '(3 4) :initial-contents '(
  87.    (0.14835072   23.38354   -4.2434387   197.26375 )
  88.    (-23.46   1.1095862   4.074173   835.20154 )
  89.    (-0.0012843199   -0.00017914758   -0.004245341   1.0))))
  90.  
  91. (defvar *j8dlt* (make-array '(3 4) :initial-contents '(
  92.    (0.55907404   24.959345   3.1993098   179.15173 )
  93.    (-17.57395   2.7324274   -16.89434   630.7562 )
  94.    (0.0026008417   0.0008867616   -0.0020301389   1.0))))
  95.  
  96.  
  97. #| OLD SET OF DLT PARAMETERS
  98. ;(defvar *j1dlt* (make-array '(3 4) :initial-contents '(
  99. ;   ( -19.8644  0.178765  -13.5437  838.773)
  100. ;   ( -3.34086  -23.3862  4.15149  1114.19)
  101. ;   (  0.001829  -0.00128091  -0.0035462  1.0))))
  102. ;
  103. ;(defvar *j2dlt* (make-array '(3 4) :initial-contents '(
  104. ;   ( -1.57888  -19.5509  12.7574  1075.15)
  105. ;   ( 23.0743  0.150066  3.52646  193.678)
  106. ;   (  0.000853097  -0.00259802  -0.00302583  1.0))))
  107. ;
  108. ;(defvar *j3dlt* (make-array '(3 4) :initial-contents '(
  109. ;   ( -23.9259  -1.6004  -4.50425  930.103)
  110. ;   ( 1.48448  -24.1476  -2.05204  977.122)
  111. ;   (  0.000155764  -0.000260534  -0.00601162  1.0))))
  112. ;
  113. ;(defvar *j4dlt* (make-array '(3 4) :initial-contents '(
  114. ;   ( 0.614891  18.9815  -13.7196  188.488)
  115. ;   ( -23.3171  -0.260518  -1.74393  808.377)
  116. ;   (  -0.0000450793  -0.00184546  -0.00320554  1.0))))
  117. ;
  118. ;(defvar *j5dlt* (make-array '(3 4) :initial-contents '(
  119. ;   ( -0.622512  23.2626  9.83324  314.566)
  120. ;   ( -25.1722  0.113823  -1.18896  760.496)
  121. ;   (  -0.000209399  0.00216168  -0.00382999  1.0))))
  122. ;
  123. ;(defvar *j6dlt* (make-array '(3 4) :initial-contents '(
  124. ;   ( 2.42274  23.1361  -6.85935  146.434)
  125. ;   ( -23.2752  0.753822  -6.56249  760.988)
  126. ;   (  0.000729179  -0.000783663  -0.00408937  1.0))))
  127. ;
  128. ;(defvar *j7dlt* (make-array '(3 4) :initial-contents '(
  129. ;   ( 0.226375  23.4827  -4.05651  197.787)
  130. ;   ( -23.4512  1.12341  3.73856  831.082)
  131. ;   (  -0.00103381  -0.000201776  -0.00401774  1.0))))
  132. ;
  133. ;(defvar *j8dlt* (make-array '(3 4) :initial-contents '(
  134. ;   ( 0.611087  24.8615  2.77736  179.376)
  135. ;   ( -17.5396  2.63451  -17.5725  628.414)
  136. ;   (  0.00272433  0.000748291  -0.00312889  1.0))))
  137. |#
  138.  
  139. (defsubst pt-to-nvec (x y)
  140.   (la:normalize x y 1.0))
  141.  
  142. (defsubst nvec-to-pt (nvec)
  143.   (list (/ (first nvec) (third nvec)) (/ (second nvec) (third nvec))))
  144.  
  145. (defsubst nvec-to-pt-values (nvec)
  146.   (values (/ (first nvec) (third nvec)) (/ (second nvec) (third nvec))))
  147.  
  148. (defsubst nvec-to-line (nvec)
  149.   (la:normalize2 nvec))
  150.  
  151. (defsubst endpoints-to-line (pt1 pt2)
  152.   (nvec-to-line (la:cross-product (list (car pt1) (cadr pt1) 1.0) (list (car pt2) (cadr pt2) 1.0))))
  153.  
  154. (defsubst pt-vec-to-line (pt vec)
  155.   (nvec-to-line (la:cross-product (list (car pt) (cadr pt) 1.0) vec)))
  156.  
  157. (defsubst line-intersection-pt (nvec1 nvec2)
  158.   (nvec-to-pt (la:cross-product nvec1 nvec2)))
  159.  
  160.  
  161. ;;;;****************************************************************
  162. ;;;; READING INPUT FILES AND INITIALIZATION
  163.  
  164. (defun read-raw-data-lines 
  165.        (tksname numrows numcols label &optional (filename "j1-lines-data.dat")
  166.     (directory "cicero:/home/vidi/vis_radius_common/MatcherData/"))
  167.   "Read a file of data lines in row1 col1 row2 col2 format, and store in x1 y1 x2 y2 format"
  168.   (with-open-file (file (format nil "~a~a" directory filename) :direction :input)
  169.     (isr2:create tksname 
  170.     :token-features 
  171.         '((x1 "" :real)(y1 "" :real)(x2 "" :real)(y2 "" :real))
  172.     :frame-features
  173.         '((numrows "" :integer) (numcols "" :integer) (label "" :string)))
  174.     (setf (isr2:value (isr2:handle (list tksname 'numrows))) (round numrows))
  175.     (setf (isr2:value (isr2:handle (list tksname 'numcols))) (round numcols))
  176.     (setf (isr2:value (isr2:handle (list tksname 'label))) label)    
  177.     (do ((row1 (read file nil :eof) (read file nil :eof)))
  178.     ((eq row1 :eof) tksname)
  179.       (let ((col1 (read file))
  180.         (row2 (read file))
  181.         (col2 (read file)))
  182.     (read file)   ;;ignore drawproc command
  183.     (let ((newtok (isr2:create-new-token tksname)))
  184.       (setf (isr2:value (list newtok 'x1)) (coerce col1 'single-float))
  185.       (setf (isr2:value (list newtok 'y1)) (coerce (- numrows row1) 'single-float))
  186.       (setf (isr2:value (list newtok 'x2)) (coerce col2 'single-float))
  187.       (setf (isr2:value (list newtok 'y2)) (coerce (- numrows row2) 'single-float)))))))
  188.  
  189.  
  190. (defun convert-raw (&optional (directory "cicero:/home/vidi/vis_radius_common/MatcherData/"))
  191.   (read-raw-data-lines 'j1lines 1039 1306 "J1" "j1-lines-data.dat" directory)
  192.   (isr2:write-frame 'j1lines "york:rcollins;j1-data-lines.isr2" :all :all)
  193.   (read-raw-data-lines 'j2lines 1037 1304 "J2" "j2-lines-data.dat" directory)
  194.   (isr2:write-frame 'j2lines "york:rcollins;j2-data-lines.isr2" :all :all)
  195.   (read-raw-data-lines 'j3lines 1041 1312 "J3" "j3-lines-data.dat" directory)
  196.   (isr2:write-frame 'j3lines "york:rcollins;j3-data-lines.isr2" :all :all)
  197.   (read-raw-data-lines 'j4lines 1041 1304 "J4" "j4-lines-data.dat" directory)
  198.   (isr2:write-frame 'j4lines "york:rcollins;j4-data-lines.isr2" :all :all)
  199.   (read-raw-data-lines 'j5lines 1039 1316 "J5" "j5-lines-data.dat" directory)
  200.   (isr2:write-frame 'j5lines "york:rcollins;j5-data-lines.isr2" :all :all)
  201.   (read-raw-data-lines 'j6lines 1038 1308 "J6" "j6-lines-data.dat" directory)
  202.   (isr2:write-frame 'j6lines "york:rcollins;j6-data-lines.isr2" :all :all)
  203.   (read-raw-data-lines 'j7lines 1043 1313 "J7" "j7-lines-data.dat" directory)
  204.   (isr2:write-frame 'j7lines "york:rcollins;j7-data-lines.isr2" :all :all)
  205.   (read-raw-data-lines 'j8lines 1032 1303 "J8" "j8-lines-data.dat" directory)
  206.   (isr2:write-frame 'j8lines "york:rcollins;j8-data-lines.isr2" :all :all)
  207.   )
  208.  
  209. (defun convert-boldt-raw (&optional (directory "cicero:/users1/vis/rcollins/Boldt/"))
  210.   (read-raw-data-lines 'j1boldt 1039 1306 "J1b Boldt lines" "j1/j1bboldt.asc" directory)
  211.   (isr2:write-frame 'j1boldt "york:rcollins;j1bboldt.isr2" :all :all)
  212.   (read-raw-data-lines 'j2boldt 1037 1304 "J2b Boldt lines" "j2/j2bboldt.asc" directory)
  213.   (isr2:write-frame 'j2boldt "york:rcollins;j2bboldt.isr2" :all :all)
  214.   (read-raw-data-lines 'j3boldt 1041 1312 "J3b Boldt lines" "j3/j3bboldt.asc" directory)
  215.   (isr2:write-frame 'j3boldt "york:rcollins;j3bboldt.isr2" :all :all)
  216.   (read-raw-data-lines 'j4boldt 1041 1304 "J4b Boldt lines" "j4/j4bboldt.asc" directory)
  217.   (isr2:write-frame 'j4boldt "york:rcollins;j4bboldt.isr2" :all :all)
  218.   (read-raw-data-lines 'j5boldt 1039 1316 "J5b Boldt lines" "j5/j5bboldt.asc" directory)
  219.   (isr2:write-frame 'j5boldt "york:rcollins;j5bboldt.isr2" :all :all)
  220.   (read-raw-data-lines 'j6boldt 1038 1308 "J6b Boldt lines" "j6/j6bboldt.asc" directory)
  221.   (isr2:write-frame 'j6boldt "york:rcollins;j6bboldt.isr2" :all :all)
  222.   (read-raw-data-lines 'j7boldt 1043 1313 "J7b Boldt lines" "j7/j7bboldt.asc" directory)
  223.   (isr2:write-frame 'j7boldt "york:rcollins;j7bboldt.isr2" :all :all)
  224.   (read-raw-data-lines 'j8boldt 1032 1303 "J8b Boldt lines" "j8/j8bboldt.asc" directory)
  225.   (isr2:write-frame 'j8boldt "york:rcollins;j8bboldt.isr2" :all :all)
  226.   )
  227.  
  228. (defun gridify-data-lines (tksname &key (rowsize 32) (colsize 32))
  229.   ;;note: I'm calling make-grid with cols and rows switched, because later I will
  230.   ;;access it using (x y) pairs rather than (row col) pairs.
  231.   (let* ((grid (isr2:make-grid 
  232.          tksname
  233.          0 (isr2:value (isr2:handle (list tksname 'numcols))) colsize
  234.          0 (isr2:value (isr2:handle (list tksname 'numrows))) rowsize))
  235.      (gss (isr2:grid-gss grid)))
  236.     (isr2:for-every-token (tok tksname (x1 y1 x2 y2))
  237.       (isr2:grid-unselect gss)
  238.       (isr2:rasterize-line gss (isr2:value x1) (isr2:value y1) (isr2:value x2) (isr2:value y2))
  239.       (isr2:grid-store gss (isr2:copy-handle tok)))
  240.     grid))
  241.  
  242. #|
  243. (defun epipolar-initialize ()
  244.   (isr2:restore 'j1-data-lines "york:rcollins;j1-data-lines.isr2")
  245.   (setf *j1grid* (gridify-data-lines 'j1-data-lines))
  246.   (setf *j1numrows* (isr2:value 'j1-data-lines$numrows))
  247.   (isr2:restore 'j2-data-lines "york:rcollins;j2-data-lines.isr2")
  248.   (setf *j2grid* (gridify-data-lines 'j2-data-lines))
  249.   (setf *j2numrows* (isr2:value 'j2-data-lines$numrows))
  250.   (isr2:restore 'j3-data-lines "york:rcollins;j3-data-lines.isr2")
  251.   (setf *j3grid* (gridify-data-lines 'j3-data-lines))
  252.   (setf *j3numrows* (isr2:value 'j3-data-lines$numrows))
  253.   (isr2:restore 'j4-data-lines "york:rcollins;j4-data-lines.isr2")
  254.   (setf *j4grid* (gridify-data-lines 'j4-data-lines))
  255.   (setf *j4numrows* (isr2:value 'j4-data-lines$numrows))
  256.   (isr2:restore 'j5-data-lines "york:rcollins;j5-data-lines.isr2")
  257.   (setf *j5grid* (gridify-data-lines 'j5-data-lines))
  258.   (setf *j5numrows* (isr2:value 'j5-data-lines$numrows))
  259.   (isr2:restore 'j6-data-lines "york:rcollins;j6-data-lines.isr2")
  260.   (setf *j6grid* (gridify-data-lines 'j6-data-lines))
  261.   (setf *j6numrows* (isr2:value 'j6-data-lines$numrows))
  262.   (isr2:restore 'j7-data-lines "york:rcollins;j7-data-lines.isr2")
  263.   (setf *j7grid* (gridify-data-lines 'j7-data-lines))
  264.   (setf *j7numrows* (isr2:value 'j7-data-lines$numrows))
  265.   (isr2:restore 'j8-data-lines "york:rcollins;j8-data-lines.isr2")
  266.   (setf *j8grid* (gridify-data-lines 'j8-data-lines))
  267.   (setf *j8numrows* (isr2:value 'j8-data-lines$numrows))
  268.   t)
  269. |#
  270.  
  271. (defun init-epipolar-data ()
  272.   (isr2:restore 'j1-data-lines "york:rcollins;j1bboldt.isr2")
  273.   (setf *j1grid* (gridify-data-lines 'j1-data-lines))
  274.   (setf *j1numrows* (isr2:value 'j1-data-lines$numrows))
  275.   (isr2:restore 'j2-data-lines "york:rcollins;j2bboldt.isr2")
  276.   (setf *j2grid* (gridify-data-lines 'j2-data-lines))
  277.   (setf *j2numrows* (isr2:value 'j2-data-lines$numrows))
  278.   (isr2:restore 'j3-data-lines "york:rcollins;j3bboldt.isr2")
  279.   (setf *j3grid* (gridify-data-lines 'j3-data-lines))
  280.   (setf *j3numrows* (isr2:value 'j3-data-lines$numrows))
  281.   (isr2:restore 'j4-data-lines "york:rcollins;j4bboldt.isr2")
  282.   (setf *j4grid* (gridify-data-lines 'j4-data-lines))
  283.   (setf *j4numrows* (isr2:value 'j4-data-lines$numrows))
  284.   (isr2:restore 'j5-data-lines "york:rcollins;j5bboldt.isr2")
  285.   (setf *j5grid* (gridify-data-lines 'j5-data-lines))
  286.   (setf *j5numrows* (isr2:value 'j5-data-lines$numrows))
  287.   (isr2:restore 'j6-data-lines "york:rcollins;j6bboldt.isr2")
  288.   (setf *j6grid* (gridify-data-lines 'j6-data-lines))
  289.   (setf *j6numrows* (isr2:value 'j6-data-lines$numrows))
  290.   (isr2:restore 'j7-data-lines "york:rcollins;j7bboldt.isr2")
  291.   (setf *j7grid* (gridify-data-lines 'j7-data-lines))
  292.   (setf *j7numrows* (isr2:value 'j7-data-lines$numrows))
  293.   (isr2:restore 'j8-data-lines "york:rcollins;j8bboldt.isr2")
  294.   (setf *j8grid* (gridify-data-lines 'j8-data-lines))
  295.   (setf *j8numrows* (isr2:value 'j8-data-lines$numrows))
  296.   t)
  297.  
  298. (defun init-epipolar-histograms (&optional (numbuckets *num-z-buckets*) (lowvalue *lowz*) (highvalue *highz*))
  299.   (setf *current-histogram* (init-height-histogram numbuckets lowvalue highvalue))
  300.   (setf *accum-histogram* (init-height-histogram numbuckets lowvalue highvalue))
  301.   (setf *spline-xarr* (make-array numbuckets :element-type 'single-float :initial-element 0.0))
  302.   (setf *spline-yarr* (make-array numbuckets :element-type 'single-float :initial-element 0.0))
  303.   t)
  304.  
  305. (defsubst set-label (window string)
  306.   (unless *no-labels*
  307.     (send window :set-label string)))
  308.  
  309. (defun init-epipolar-screens ()
  310.   (setf *display1* (make-instance 'sg:schema-image-display))
  311.   (send *display1* :set-screen-window 0 0 504 395)
  312.   (setf *display2* (make-instance 'sg:schema-image-display))
  313.   (send *display2* :set-screen-window 512 0 504 395)
  314.   (setf *plot1* (make-instance 'sg:schema-image-display))
  315.   (send *plot1* :set-screen-window 0 416 504 116)
  316.   (set-label *plot1* "accumulated height histogram")
  317.   (setf *plot2* (make-instance 'sg:schema-image-display))
  318.   (send *plot2* :set-screen-window 512 416 504 116)
  319.   (set-label *plot2*  "current height histogram")
  320.   t)
  321.  
  322. (defun init-epipolar-display (&key (init-screens t))
  323.   (when init-screens
  324.     (init-epipolar-screens))
  325.   (let ((w *display1*)
  326.     (w2 *display2*))
  327.     (display-line-tokenset w 'j8-data-lines :init t)
  328.     (send w :take-snapshot)
  329.     (send w :store-display 'j8lines)
  330.     (display-line-tokenset w 'j6-data-lines :init t)
  331.     (send w :take-snapshot)
  332.     (send w :store-display 'j6lines)
  333.     (display-line-tokenset w 'j3-data-lines :init t)
  334.     (send w :take-snapshot)
  335.     (send w :store-display 'j3lines)
  336.     (display-line-tokenset w2 'j8-data-lines :init t) 
  337.     (send w2 :take-snapshot)
  338.     (send w2 :store-display 'j8lines)
  339.     (display-line-tokenset w2 'j7-data-lines :init t) 
  340.     (send w2 :take-snapshot)
  341.     (send w2 :store-display 'j7lines)
  342.     (display-line-tokenset w2 'j6-data-lines :init t) 
  343.     (send w2 :take-snapshot)
  344.     (send w2 :store-display 'j6lines)
  345.     (display-line-tokenset w2 'j5-data-lines :init t) 
  346.     (send w2 :take-snapshot)
  347.     (send w2 :store-display 'j5lines)
  348.     (display-line-tokenset w2 'j4-data-lines :init t) 
  349.     (send w2 :take-snapshot)
  350.     (send w2 :store-display 'j4lines)
  351.     (display-line-tokenset w2 'j3-data-lines :init t) 
  352.     (send w2 :take-snapshot)
  353.     (send w2 :store-display 'j3lines)
  354.     (display-line-tokenset w2 'j2-data-lines :init t) 
  355.     (send w2 :take-snapshot)
  356.     (send w2 :store-display 'j2lines)
  357.     (display-line-tokenset w2 'j1-data-lines :init t) 
  358.     (send w2 :take-snapshot)
  359.     (send w2 :store-display 'j1lines)
  360.     t))
  361.  
  362. ;;;;****************************************************************
  363. ;;;; DLT - DIRECT LINEAR TRANSFORM
  364.  
  365. (defun dlt-to-amat-bvec (dlt)
  366.   (values (make-array '(3 3) :initial-contents 
  367.               (list (list (aref dlt 0 0) (aref dlt 0 1) (aref dlt 0 2))
  368.                 (list (aref dlt 1 0) (aref dlt 1 1) (aref dlt 1 2))
  369.                 (list (aref dlt 2 0) (aref dlt 2 1) (aref dlt 2 2))))
  370.       (list (aref dlt 0 3) (aref dlt 1 3) (aref dlt 2 3))))
  371.  
  372. (defun amat-bvec-to-dlt (amat bvec)
  373.   (make-array '(3 4) 
  374.     :initial-contents 
  375.     (list (list (aref amat 0 0) (aref amat 0 1) (aref amat 0 2) (elt bvec 0))
  376.       (list (aref amat 1 0) (aref amat 1 1) (aref amat 1 2) (elt bvec 1))
  377.       (list (aref amat 2 0) (aref amat 2 1) (aref amat 2 2) (elt bvec 2)))))
  378.  
  379. (defun inverse-dlt (dlt)
  380.   (multiple-value-bind (amat bvec) (dlt-to-amat-bvec dlt)
  381.     (let ((invamat (solve:inverse amat)))
  382.       (amat-bvec-to-dlt invamat (la:vs -1.0 (la:m* invamat bvec))))))
  383.  
  384.  
  385. (defvar *j1inverse* (inverse-dlt *j1dlt*))
  386. (defvar *j2inverse* (inverse-dlt *j2dlt*))
  387. (defvar *j3inverse* (inverse-dlt *j3dlt*))
  388. (defvar *j4inverse* (inverse-dlt *j4dlt*))
  389. (defvar *j5inverse* (inverse-dlt *j5dlt*))
  390. (defvar *j6inverse* (inverse-dlt *j6dlt*))
  391. (defvar *j7inverse* (inverse-dlt *j7dlt*))
  392. (defvar *j8inverse* (inverse-dlt *j8dlt*))
  393.  
  394. (defun dlt-3d-to-2d (dlt x y z)
  395.   "Projects 3D world point x y z into image plane using given dlt projective transformation matrix"
  396.   (nvec-to-pt (listarray (la:m* dlt (list x y z 1.0)))))
  397.  
  398. (defun dlt-2d-to-3d (dlt x y &optional (dlt-inv (inverse-dlt dlt)))
  399.   "Backprojects 2d image point x y into world ray using inverse of dlt projective transformation 
  400.   matrix. The resulting world ray is two vectors a and b describing the line k a + b."
  401.   (values (listarray (la:m* dlt-inv (list x y 1.0 0.0)))
  402.       (listarray (la:m* dlt-inv (list 0.0 0.0 0.0 1.0)))))
  403.  
  404.  
  405. ;;;;****************************************************************
  406. ;;;; COMPUTING THE CROSS RATIO AND SOLVING FOR HEIGHT
  407.  
  408.  
  409. ;(defun cross-ratio-of-unit-vectors (u1 u2 u3 u4)
  410. ;  "Returns cross ratio of four coplanar 3d unit vectors."
  411. ;  (let ((c13 (la:dot-product u1 u3))
  412. ;    (c14 (la:dot-product u1 u4))
  413. ;    (c23 (la:dot-product u2 u3))
  414. ;    (c24 (la:dot-product u2 u4)))
  415. ;    (print (list c13 c14 c23 c24))
  416. ;    (let ((s13sq (- 1.0 (* c13 c13)))
  417. ;      (s14sq (- 1.0 (* c14 c14)))
  418. ;      (s23sq (- 1.0 (* c23 c23)))
  419. ;      (s24sq (- 1.0 (* c24 c24))))
  420. ;      (sqrt (* (/ s13sq s23sq) (/ s24sq s14sq))))))
  421.  
  422.  
  423. (defsubst height-cross-ratio (1overcamz lowz highz zval)
  424.   (* (/ (- 1.0 (* highz 1overcamz)) (- 1.0 (* zval 1overcamz)))
  425.      (/ (- zval lowz) (- highz lowz))))
  426.  
  427. (defun cr-reference-line (lowline epi2line highline epi1line)
  428.   (let* ((midline (la:normalize2 (la:v- lowline highline)))
  429.      (epimidline (la:normalize2 (la:v- epi2line epi1line)))
  430.      (center (line-intersection-pt midline epimidline)))
  431.     (list (- (second midline)) 
  432.       (first midline)
  433.       (- (* (second midline) (car center))
  434.          (* (first midline) (cadr center))))))
  435.  
  436. (defun cr-reference-coord (cr-reference-line line)
  437.   "returns homogeneous coordinates of intersection point on cross ratio reference line"
  438.   (let ((pt (la:unit-normal line cr-reference-line))
  439.     (cos (car cr-reference-line))
  440.     (sin (cadr cr-reference-line)))
  441.     (list (- (* cos (second pt)) (* sin (first pt)))
  442.       (third pt))))
  443.  
  444. (defun cr-reference-value (cr-reference-line line &key (reciprocal? nil))
  445.   (multiple-value-bind (x y) (values-list (cr-reference-coord cr-reference-line line))
  446.     (if reciprocal?
  447.     (/ y x)
  448.     (/ x y))))
  449.  
  450. (defun solve-for-height-given-cr (crvalue 1overcamz lowz highz)
  451.   (let ((tmp (* crvalue (- highz lowz))))
  452.     (/ (+ tmp lowz (* (- highz) lowz 1overcamz))
  453.        (+ 1.0 (* (- tmp highz) 1overcamz)))))
  454.  
  455. (defun solve-for-cr-given-height (heightvalue 1overcamz lowz highz)
  456.   (height-cross-ratio 1overcamz lowz highz heightvalue))
  457.  
  458.  
  459. ;;;;****************************************************************
  460. ;;;; HISTOGRAMMING HEIGHT VALUES
  461.  
  462. (defun init-height-histogram (&optional (numbuckets *num-z-buckets*) (lowvalue *lowz*) (highvalue *highz*))
  463.   (hist:make-histogram :num-buckets numbuckets :min-value lowvalue :max-value highvalue))
  464.  
  465. (defun clear-height-histogram (&optional (histogram *current-histogram*))
  466.   (let ((arr (hist:histogram-array histogram)))
  467.     (dotimes (i (length arr) histogram)
  468.       (setf (aref arr i) 0.0))))
  469.  
  470. (defun add-to-height-histogram (hist1 hist2)
  471.   "destructively add hist2 into hist1"
  472.   (let ((arr1 (hist:histogram-array hist1))
  473.     (arr2 (hist:histogram-array hist2)))
  474.     (dotimes (i (length arr1) hist2)
  475.       (incf (aref arr1 i) (aref arr2 i)))))
  476.  
  477. (defsubst cross-ratio-CDF (crvalue crmin crmax)
  478.   (cond ((<= crvalue crmin) 0.0)
  479.     ((< crvalue crmax) (/ (- crvalue crmin) (- crmax crmin)))
  480.     (t 1.0)))
  481.  
  482. (defun probability-of-cross-ratio-range  (cra crb crmin crmax)
  483.   (abs (- (cross-ratio-CDF crb crmin crmax)
  484.       (cross-ratio-CDF cra crmin crmax))))
  485.  
  486. (defun vote-for-height-range (cr1 cr2 1overcamz lowz highz &key (weight 1) (histogram *current-histogram*))
  487.   (when (< cr2 cr1) (rotatef cr1 cr2))
  488.   (let ((height1 (solve-for-height-given-cr cr1 1overcamz lowz highz))
  489.     (height2 (solve-for-height-given-cr cr2 1overcamz lowz highz))
  490.     (arr (hist:histogram-array histogram)))
  491.     (when (< height2 height1) (rotatef height1 height2))
  492.     (let ((lowindex (hist:value-to-index histogram height1))
  493.       (highindex (hist:value-to-index histogram height2)))
  494.       (do  ((index lowindex (+ 1 index)))
  495.        ((> index highindex) (values height1 height2))
  496.         (multiple-value-bind (rangelow rangehigh) (values-list (hist:index-to-range histogram index))
  497.           (unless rangelow (setf rangelow lowz))
  498.           (unless rangehigh (setf rangehigh highz))
  499.           (let ((cra (solve-for-cr-given-height rangelow 1overcamz lowz highz))
  500.             (crb (solve-for-cr-given-height rangehigh 1overcamz lowz highz)))
  501.         (incf (aref arr index)
  502.               (* weight (probability-of-cross-ratio-range cra crb cr1 cr2)))))))))
  503.  
  504.  
  505. ;;;;****************************************************************
  506. ;;;; SEARCHING FOR CANDIDATE EPIPOLAR MATCHES
  507.  
  508. (defun coarse-get-lines-in-quadrilateral (grid pt1 pt2 pt3 pt4)
  509.   (let ((gss (isr2:grid-gss grid)))
  510.     (isr2:grid-unselect gss)
  511.     (isr2:rasterize-polygon gss (list pt1 pt2 pt3 pt4))
  512.     (isr2:grid-retrieve gss)))
  513.  
  514. (defun backproject-point (dlt-inverse x y zval)
  515.   "Backproject image point x y and return the 3d point where the backprojection ray
  516.   intersects the plane z = zval."
  517.   (multiple-value-bind (a b) (dlt-2d-to-3d nil x y dlt-inverse)
  518.     (let ((k (/ (- zval (third b)) (third a))))
  519.       (la:v+ b (la:vs k a)))))
  520.  
  521. (defun epipolar-search-area (dlt1-inverse dlt2 x1 y1 x2 y2  &optional (zlow *lowz*) (zhigh *highz*))
  522.   (list (apply #'dlt-3d-to-2d dlt2 (backproject-point dlt1-inverse x1 y1 zlow))
  523.     (apply #'dlt-3d-to-2d dlt2 (backproject-point dlt1-inverse x2 y2 zlow))
  524.     (apply #'dlt-3d-to-2d dlt2 (backproject-point dlt1-inverse x1 y1 zhigh))
  525.     (apply #'dlt-3d-to-2d dlt2 (backproject-point dlt1-inverse x2 y2 zhigh))))
  526.  
  527. (defun sloppy-points (pt1 pt2 trueline endpoint-slop)
  528.   (let ((x1 (car pt1))
  529.     (y1 (cadr pt1))
  530.     (x2 (car pt2))
  531.     (y2 (cadr pt2))
  532.     (slopx (* (car trueline) endpoint-slop))
  533.     (slopy (* (cadr trueline) endpoint-slop)))
  534.     (if (minusp (la:dot-product trueline (pt-to-nvec x1 y1)))
  535.     (values
  536.       (list (- x1 slopx) (- y1 slopy))
  537.       (list (+ x2 slopx) (+ y2 slopy)))
  538.     (values
  539.       (list (+ x1 slopx) (+ y1 slopy))
  540.       (list (- x2 slopx) (- y2 slopy))))))
  541.  
  542. (defun histogram-epipolar-candidates (w grid low1 low2 high1 high2 numrows
  543.         &key (deltatheta .1) (relative-length-thresh .25) (histogram *current-histogram*)
  544.         (endpoint-slop 1) (prompt t))
  545.   (multiple-value-bind (lowline epi2line highline epi1line)
  546.       (clip-to-quad-init low1 low2 high2 high1)
  547. ;    (setf *lowline* lowline *epi2line* epi2line *highline* highline *epi1line* epi1line)
  548.     (let* ((vp (la:unit-normal lowline highline))
  549.        (epi (la:unit-normal epi1line epi2line))
  550.        (vp-epi-line (la:unit-normal vp epi))
  551.        (cr-reference-line (cr-reference-line lowline epi2line highline epi1line))
  552.        (1overcamz (cr-reference-value cr-reference-line vp-epi-line :reciprocal? t))
  553.        (lowz (cr-reference-value cr-reference-line lowline))
  554.        (highz (cr-reference-value cr-reference-line highline))
  555.        (cos-theta-thresh (cos (abs deltatheta)))
  556.        (candidates nil) (line nil) (trueline nil) (relative-len nil))
  557.       (setf candidates (coarse-get-lines-in-quadrilateral grid low1 low2 high2 high1))
  558.       (when *demo-mode*
  559.     (epipolar-display w low1 low2 high1 high2 numrows :zoom t)
  560. ;       (let ((pt1 (line-intersection-pt cr-reference-line lowline))
  561. ;         (pt2 (line-intersection-pt cr-reference-line highline)))
  562. ;     (display-line w (car pt1) (cadr pt1) (car pt2) (cadr pt2) numrows :color w:green)
  563. ;     (send w :display-point (car pt2) (- numrows (cadr pt2)) :radius 2 :color w:green))
  564.     (display-line-tokenset w candidates :color *line-color* :thickness *line-thickness*)
  565.     (epipolar-display w low1 low2 high1 high2 numrows :zoom nil))
  566.       (isr2:for-every-token (tok candidates (x1 y1 x2 y2))
  567.     (multiple-value-bind (x1 y1 x2 y2 ok?)
  568.         (clip-to-quad (isr2:value x1) (isr2:value y1) (isr2:value x2) (isr2:value y2)
  569.               lowline epi2line highline epi1line)
  570.       (when ok?
  571.         (let ((pt1 (list x1 y1))
  572.           (pt2 (list x2 y2))
  573.           (midpt (list (/ (+ x1 x2) 2.0) (/ (+ y1 y2) 2.0))))
  574.           (setf line (endpoints-to-line pt1 pt2))
  575.           (setf trueline (pt-vec-to-line midpt vp))
  576.           (when (< cos-theta-thresh (abs (la:dot-product (firstn 2 line) (firstn 2 trueline))))
  577.         (setf relative-len 
  578.               (/ (vp:line-length x1 y1 x2 y2)
  579.              (apply #'(lambda (x1 y1 x2 y2) (vp:line-length x1 y1 x2 y2))
  580.                 (append (line-intersection-pt trueline epi1line)
  581.                     (line-intersection-pt trueline epi2line)))))
  582.         (when (>= relative-len relative-length-thresh)
  583.           (multiple-value-setq (pt1 pt2) (sloppy-points pt1 pt2 trueline endpoint-slop))
  584.           (let ((zvalue1 (cr-reference-value cr-reference-line (pt-vec-to-line pt1 vp)))
  585.             (zvalue2 (cr-reference-value cr-reference-line (pt-vec-to-line pt2 vp))))
  586.             (multiple-value-bind (lowheight highheight)
  587.             (vote-for-height-range 
  588.               (height-cross-ratio 1overcamz lowz highz zvalue1)
  589.               (height-cross-ratio 1overcamz lowz highz zvalue2)
  590.               *1overcamz* *lowz* *highz*
  591.               :weight relative-len
  592.               :histogram histogram)
  593.               (when prompt
  594.             (format t "Token ~s votes ~d for height range (~d,~d)~%" 
  595.                 tok relative-len lowheight highheight)))
  596.             (when *demo-mode*
  597. ;               (display-line w (car pt1) (cadr pt1) (car pt2) (cadr pt2) numrows
  598. ;                    :color w:red :thickness 3)
  599.               (display-line w x1 y1 x2 y2 numrows :color *search-color* :thickness *search-thickness*))
  600.             ()))))))))))
  601.  
  602.  
  603.  
  604. (defun test-single-epipolar-candidate (w grid low1 low2 high1 high2 numrows testx1 testy1 testx2 testy2
  605.         &key (deltatheta .1) (relative-length-thresh .25) (histogram *current-histogram*)
  606.         (endpoint-slop 1))
  607.   (multiple-value-bind (lowline epi2line highline epi1line)
  608.       (clip-to-quad-init low1 low2 high2 high1)
  609. ;    (setf *lowline* lowline *epi2line* epi2line *highline* highline *epi1line* epi1line)
  610.     (let* ((vp (la:unit-normal lowline highline))
  611.        (epi (la:unit-normal epi1line epi2line))
  612.        (vp-epi-line (la:unit-normal vp epi))
  613.        (cr-reference-line (cr-reference-line lowline epi2line highline epi1line))
  614.        (1overcamz (cr-reference-value cr-reference-line vp-epi-line :reciprocal? t))
  615.        (lowz (cr-reference-value cr-reference-line lowline))
  616.        (highz (cr-reference-value cr-reference-line highline))
  617.        (cos-theta-thresh (cos (abs deltatheta)))
  618.        (candidates nil) (line nil) (trueline nil) (relative-len nil))
  619.       (setf candidates (coarse-get-lines-in-quadrilateral grid low1 low2 high2 high1))
  620.       (epipolar-display w low1 low2 high1 high2 numrows :zoom t)
  621.       (let ((pt1 (line-intersection-pt cr-reference-line lowline))
  622.         (pt2 (line-intersection-pt cr-reference-line highline)))
  623.     (display-line w (car pt1) (cadr pt1) (car pt2) (cadr pt2) numrows :color w:green :thickness 3)
  624.     (send w :display-point (car pt2) (- numrows (cadr pt2)) :radius 2 :color w:green))
  625.       (display-line-tokenset w candidates)
  626.       (multiple-value-bind (x1 y1 x2 y2 ok?)
  627.       (clip-to-quad testx1 testy1 testx2 testy2 lowline epi2line highline epi1line)
  628.     (when ok?
  629.       (let ((pt1 (list x1 y1))
  630.         (pt2 (list x2 y2))
  631.         (midpt (list (/ (+ x1 x2) 2.0) (/ (+ y1 y2) 2.0))))
  632.         (setf line (endpoints-to-line pt1 pt2))
  633.         (setf trueline (pt-vec-to-line midpt vp))
  634.         (when (< cos-theta-thresh (abs (la:dot-product (firstn 2 line) (firstn 2 trueline))))
  635.           (setf relative-len 
  636.             (/ (vp:line-length x1 y1 x2 y2)
  637.                (apply #'(lambda (x1 y1 x2 y2) (vp:line-length x1 y1 x2 y2))
  638.                   (append (line-intersection-pt trueline epi1line)
  639.                       (line-intersection-pt trueline epi2line)))))
  640.           (when (>= relative-len relative-length-thresh)
  641.         (multiple-value-setq (pt1 pt2) (sloppy-points pt1 pt2 trueline endpoint-slop))
  642.         (let ((zvalue1 (cr-reference-value cr-reference-line (pt-vec-to-line pt1 vp)))
  643.               (zvalue2 (cr-reference-value cr-reference-line (pt-vec-to-line pt2 vp))))
  644.           (print (list 1overcamz lowz highz zvalue1))
  645.           (print (list 1overcamz lowz highz zvalue2))
  646.           (multiple-value-bind (lowheight highheight)
  647.               (vote-for-height-range 
  648.             (height-cross-ratio 1overcamz lowz highz zvalue1)
  649.             (height-cross-ratio 1overcamz lowz highz zvalue2)
  650.             *1overcamz* *lowz* *highz*
  651.             :weight relative-len
  652.             :histogram histogram)
  653.             (format t "Token ~s votes ~d for height range (~d,~d)~%" 
  654.                 (list testx1 testy1 testx2 testy2) relative-len lowheight highheight)
  655.             (format t "cross-ratios ~d ~d~%"
  656.                 (height-cross-ratio 1overcamz lowz highz zvalue1)
  657.                 (height-cross-ratio 1overcamz lowz highz zvalue2))
  658.             )          
  659.           (display-line w x1 y1 x2 y2 numrows :color w:blue)
  660.           (display-line w (car pt1) (cadr pt1) (car pt2) (cadr pt2) numrows :color w:red)
  661.           ())))))))))
  662.  
  663. (defun test-epipolar-search-area (w dlt1-inverse dlt2 x1 y1 x2 y2 grid numrows &key
  664.                   (deltatheta .1) (relative-length-thresh .25)
  665.                   (endpoint-slop 1) (testline nil) (prompt t))
  666.   (multiple-value-bind (low1 low2 high1 high2)
  667.       (values-list (epipolar-search-area dlt1-inverse dlt2 x1 y1 x2 y2))
  668.     (when prompt
  669.       (epipolar-display w low1 low2 high1 high2 numrows)
  670.       (yes-or-no-p "continue"))
  671.     (if testline
  672.     (test-single-epipolar-candidate 
  673.       w grid low1 low2 high1 high2 numrows
  674.       (car testline) (cadr testline) (caddr testline) (fourth testline)
  675.       :deltatheta deltatheta
  676.       :relative-length-thresh relative-length-thresh
  677.       :endpoint-slop endpoint-slop)
  678.     (histogram-epipolar-candidates 
  679.       w grid low1 low2 high1 high2 numrows
  680.       :deltatheta deltatheta
  681.       :relative-length-thresh relative-length-thresh
  682.       :endpoint-slop endpoint-slop :prompt prompt))))
  683.  
  684. (defun test-all-epipolar-search-areas (linelist &key (endpoint-slop 1.0)(delta-theta .1)
  685.                        (prompt t) (prompt-first nil))
  686.   (let ((w *display1*)
  687.     (w2 *display2*)
  688.     (first t))
  689.     (clear-height-histogram *accum-histogram*)
  690.     (when *demo-mode*
  691.       (send *plot1* :clear)
  692.       (send *plot2* :clear))
  693.     (dolist (line linelist)
  694.       (multiple-value-bind (x1 y1 x2 y2) (values-list line)
  695.     (when *demo-mode*
  696.       (send w :restore-display 'j3lines)
  697.       (display-line w x1 y1 x2 y2 *j3numrows* :color *search-color* :thickness (1+ *search-thickness*)))
  698.     (mapcar
  699.       #'(lambda (tks displayname dlt grid numrows)
  700.           (when *demo-mode*
  701.         (let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
  702.               (numcols (isr2:value (isr2:handle (list tks 'numcols))))
  703.               (label (isr2:value (isr2:handle (list tks 'label)))))
  704.           (set-label w2 label)
  705.           (send w2 :set-image-window 0 0 numcols numrows)))
  706.           (when (and prompt *demo-mode*)
  707.         (send w2 :restore-display displayname))
  708.           (clear-height-histogram *current-histogram*)
  709.           (test-epipolar-search-area w2 *j3inverse* dlt x1 y1 x2 y2 grid numrows
  710.                      :deltatheta delta-theta :relative-length-thresh .1
  711.                      :endpoint-slop endpoint-slop :prompt prompt)
  712.           (when *demo-mode*
  713.         (plot-histogram *plot2* *current-histogram* :max 1))
  714.           (add-to-height-histogram *accum-histogram* *current-histogram*)
  715.           (when *demo-mode*
  716.         (plot-histogram *plot1* *accum-histogram* :max 3))
  717.           (when (and first prompt-first)
  718.         (yes-or-no-p "continue"))
  719.           (setf first nil)        
  720.           (when prompt
  721.         (do ((done (yes-or-no-p "next image") (yes-or-no-p "next image")))
  722.             (done t)
  723.           (print "enter line coordinate list (x1 y1 x2 y2)")
  724.           (let ((testline (eval (read))))
  725.             (test-epipolar-search-area w2 *j3inverse* dlt x1 y1 x2 y2 grid numrows
  726.                            :deltatheta delta-theta :relative-length-thresh .1 
  727.                            :testline testline
  728.                            :endpoint-slop endpoint-slop)))))
  729.       (list 'j1-data-lines 'j2-data-lines 'j4-data-lines 
  730.         'j5-data-lines 'j6-data-lines 'j7-data-lines 'j8-data-lines)
  731.       (list 'j1lines 'j2lines 'j4lines 'j5lines 'j6lines 'j7lines 'j8lines)
  732.       (list *j1dlt* *j2dlt* *j4dlt* *j5dlt* *j6dlt* *j7dlt* *j8dlt*) 
  733.       (list *j1grid* *j2grid* *j4grid* *j5grid* *j6grid* *j7grid* *j8grid*)
  734.       (list *j1numrows* *j2numrows* *j4numrows* *j5numrows* *j6numrows* *j7numrows* *j8numrows*))))
  735.     *accum-histogram*))
  736.  
  737.  
  738. ;;;;****************************************************************
  739. ;;;; CLIPPING TO A QUADRILATERAL
  740.  
  741.  
  742. (defsubst ccw-p (x1 y1 x2 y2 x3 y3)
  743.   "  Returns T iff (x1,y1)-(x2,y2)-(x3,y3)-(x1,y1) is a counterclockwise walk, NIL otherwise."
  744. ;;;;Note: computed by taking the determinant of the matrix
  745. ;;;;   x1 y1 1 
  746. ;;;;   x2 y2 1 
  747. ;;;;   x3 y3 1 
  748. ;;;;the determinant is positive iff p1-p2-p3 form a CCW cycle
  749.   (plusp (+ (* x1 (- y2 y3)) 
  750.         (* y1 (- x3 x2)) 
  751.         (- (* x2 y3) (* y2 x3)))))
  752.  
  753. (defun clip-to-quad-init (pt1 pt2 pt3 pt4)
  754.   "  Initialize the four homogeneous coordinate line vectors in the form needed
  755.   by clip-to-quad-aux.  The given points must represent a walk around the quadrilateral."
  756.   (unless (apply #'ccw-p (append pt1 pt2 pt3))
  757.     (rotatef pt1 pt2)
  758.     (rotatef pt3 pt4))
  759.   (values (endpoints-to-line pt1 pt2)  ;\
  760.       (endpoints-to-line pt2 pt3)  ; \ a CCW walk around the quadrilateral
  761.       (endpoints-to-line pt3 pt4)  ; /
  762.       (endpoints-to-line pt4 pt1)  ;/
  763.       ))
  764.  
  765. (defconstant topclipcode 8)
  766. (defconstant botclipcode 4)
  767. (defconstant rightclipcode 2)
  768. (defconstant leftclipcode 1)
  769.  
  770.  
  771. ;(defparameter *small-negative-distance* -1e-6)
  772. ;
  773. ;(defsubst clip-to-quad-outcodes (x y top left bot right)
  774. ;  (let ((vec (list x y 1.0))) 
  775. ;    (print (list x y (la:dot-product top vec) (la:dot-product bot vec) 
  776. ;                     (la:dot-product right vec) (la:dot-product left vec)))
  777. ;    (+
  778. ;      (if (< (la:dot-product top vec) *small-negative-distance*) topclipcode 0)
  779. ;      (if (< (la:dot-product bot vec) *small-negative-distance*) botclipcode 0)
  780. ;      (if (< (la:dot-product right vec) *small-negative-distance*) rightclipcode 0)
  781. ;      (if (< (la:dot-product left vec) *small-negative-distance*) leftclipcode 0))))
  782.  
  783.  
  784. (defsubst clip-to-quad-outcodes (x y top left bot right)
  785.   (let ((vec (list x y 1.0))) 
  786. ;    (print (list x y (la:dot-product top vec) (la:dot-product bot vec) 
  787. ;                     (la:dot-product right vec) (la:dot-product left vec)))
  788.     (+
  789.       (if (minusp (la:dot-product top vec)) topclipcode 0)
  790.       (if (minusp (la:dot-product bot vec)) botclipcode 0)
  791.       (if (minusp (la:dot-product right vec)) rightclipcode 0)
  792.       (if (minusp (la:dot-product left vec)) leftclipcode 0))))
  793.  
  794. (defsubst clip-to-quad-line-intersect (line clipline)
  795.   (nvec-to-pt-values (la:cross-product line clipline)))
  796.  
  797. ;(defsubst clip-to-quad-line-intersect (line clipline)
  798. ;  (values-list (listarray 
  799. ;         (solve:linear-system 
  800. ;           (make-array '(2 2) :initial-contents (list (firstn 2 line) (firstn 2 clipline)))
  801. ;           (vector (- (third line)) (- (third clipline)))))))
  802.  
  803. (defsubst clip-to-quad-reject-check (outcode1 outcode2)
  804.   (not (zerop (logand outcode1 outcode2))))
  805.  
  806. (defsubst clip-to-quad-accept-check (outcode1 outcode2)
  807.   (and (zerop outcode1) (zerop outcode2)))
  808.  
  809. (defun clip-to-quad (x1 y1 x2 y2 top left bot right)
  810.   "Clip line x1 y1 x2 y2 to a quadrilateral defined by homogeneous coordinate
  811.   lines top,left,bot,right which are calculated by clip-to-quad-init."
  812.   (let ((accept nil) (reject nil) (ignorecode1 0) (ignorecode2 0)
  813.     (line (endpoints-to-line (list x1 y1) (list x2 y2))))
  814. ;    (print (list x1 y1 x2 y2 top left bot right))
  815. ;    (multiple-value-setq (pt1 pt2) (values (list x1 y1) (list x2 y2)))
  816. ;    (multiple-value-setq (l1 l2 l3 l4) (values top bot right left))
  817.     (do ((done nil))
  818.     (done t)  ;;loop until done
  819.       (let ((outcode1 (logandc1 ignorecode1 (clip-to-quad-outcodes x1 y1 top left bot right)))
  820.         (outcode2 (logandc1 ignorecode2 (clip-to-quad-outcodes x2 y2 top left bot right))))
  821. ;    (print (list outcode1 outcode2))
  822.     (setf reject (clip-to-quad-reject-check outcode1 outcode2))
  823.     (if reject
  824.         (setf done t)
  825.         (progn
  826.           (setf accept (clip-to-quad-accept-check outcode1 outcode2))
  827.           (if accept
  828.           (setf done t)
  829.           (progn
  830.             (when (= outcode1 0)
  831.               (rotatef x1 x2)
  832.               (rotatef y1 y2)
  833.               (rotatef outcode1 outcode2)
  834.               (rotatef ignorecode1 ignorecode2))
  835.             (cond
  836.               ((logtest topclipcode outcode1)
  837.                (multiple-value-setq (x1 y1) (clip-to-quad-line-intersect line top))
  838.                (incf ignorecode1 topclipcode))
  839.               ((logtest botclipcode outcode1)
  840.                (multiple-value-setq (x1 y1) (clip-to-quad-line-intersect line bot))
  841.                (incf ignorecode1 botclipcode))
  842.               ((logtest rightclipcode outcode1)
  843.                (multiple-value-setq (x1 y1) (clip-to-quad-line-intersect line right))
  844.                (incf ignorecode1 rightclipcode))
  845.               ((logtest leftclipcode outcode1)
  846.                (multiple-value-setq (x1 y1) (clip-to-quad-line-intersect line left))
  847.                (incf ignorecode1 leftclipcode)))))))))
  848.     (if accept
  849.     (values x1 y1 x2 y2 t)
  850.     (values 0 0 0 0 nil))))
  851.  
  852.  
  853. (defun test-clip-to-quad (w pt1 pt2 pt3 pt4)
  854.   (declare (special l1 l2 l3 l4))
  855.   (send w :clear)
  856.   (send w :display-line (car pt1) (cadr pt1) (car pt2) (cadr pt2) :color w:black)
  857.   (send w :display-line (car pt2) (cadr pt2) (car pt3) (cadr pt3) :color w:black)
  858.   (send w :display-line (car pt3) (cadr pt3) (car pt4) (cadr pt4) :color w:black)
  859.   (send w :display-line (car pt4) (cadr pt4) (car pt1) (cadr pt1) :color w:black)
  860.   (multiple-value-setq (l1 l2 l3 l4)
  861.     (clip-to-quad-init pt1 pt2 pt3 pt4))
  862.   (loop
  863.     (let ((input (list (random 10.0)  (random 10.0)  (random 10.0)  (random 10.0))))
  864.       (multiple-value-bind (x1 y1 x2 y2 ok)
  865.       (clip-to-quad (car input) (cadr input) (third input) (fourth input) l1 l2 l3 l4)
  866.     (when ok
  867.       (send w :display-line x1 y1 x2 y2 :color w:green))))))
  868.  
  869. ;;;;****************************************************************
  870. ;;;; DISPLAY ROUTINES
  871.  
  872. (defun display-line-tokenset (w tks &key (init nil) (color w:black) (thickness 1))
  873.   (setf tks (isr2:handle tks))
  874.   (let ((numrows (isr2:value (isr2:handle (list (isr2:frame tks) 'numrows)))))
  875.     (when init
  876.       (let ((numcols (isr2:value (isr2:handle (list (isr2:frame tks) 'numcols))))
  877.         (label (isr2:value (isr2:handle (list (isr2:frame tks) 'label)))))
  878.     (set-label w label)
  879.     (send w :clear)
  880.     (send w :set-image-window 0 0 numcols numrows)))
  881.     (isr2:for-every-token (tok tks (x1 y1 x2 y2))
  882.       (send w :display-line (isr2:value x1) (- numrows (isr2:value y1))
  883.         (isr2:value x2) (- numrows (isr2:value y2)) :color color :thickness thickness))))
  884.  
  885. (defun display-line-token (w token numrows &key (color w:black) (thickness 1))
  886.   (send w :display-line 
  887.     (isr2:value (list token 'x1))
  888.     (- numrows (isr2:value (list token 'y1)))
  889.     (isr2:value (list token 'x2))
  890.     (- numrows (isr2:value (list token 'y2)))
  891.     :color color :thickness thickness))
  892.  
  893. (defun display-line (w x1 y1 x2 y2 numrows &key (color w:black) (thickness 1))
  894.   (send w :display-line x1 (- numrows y1) x2 (- numrows y2) :color color :thickness thickness))
  895.  
  896. (defun epipolar-display (w low1 low2 high1 high2 numrows &key (zoom nil) (boundary-pixels 5) 
  897.              (color  *epipolar-color*) (thickness *epipolar-thickness*))
  898.   (multiple-value-bind (x1 y1) (values-list low1)
  899.   (multiple-value-bind (x2 y2) (values-list low2)
  900.   (multiple-value-bind (x3 y3) (values-list high1)
  901.   (multiple-value-bind (x4 y4) (values-list high2)
  902.     (setf y1 (- numrows y1))
  903.     (setf y2 (- numrows y2))
  904.     (setf y3 (- numrows y3))
  905.     (setf y4 (- numrows y4))
  906.     (when zoom
  907.       (send w :clear)
  908.       (let ((minx (min x1 x2 x3 x4))
  909.         (maxx (max x1 x2 x3 x4))
  910.         (miny (min y1 y2 y3 y4))
  911.         (maxy (max y1 y2 y3 y4)))
  912.     (let* ((dx (- maxx minx))
  913.            (dy (- maxy miny))
  914.            (maxd (max dx dy)))
  915.       (send w :set-image-window 
  916.         (floor (- (/ (+ minx maxx) 2) (/ maxd 2) boundary-pixels))
  917.         (floor (- (/ (+ miny maxy) 2) (/ maxd 2) boundary-pixels))
  918.         (ceiling (+ maxd (* 2 boundary-pixels)))))))
  919.     (send w :display-line x1 y1 x2 y2 :color color :thickness thickness)
  920.     (send w :display-line x2 y2 x4 y4 :color color :thickness thickness)
  921.     (send w :display-line x4 y4 x3 y3 :color color :thickness thickness)
  922.     (send w :display-line x3 y3 x1 y1 :color color :thickness thickness))))))
  923.  
  924. (defun plot-histogram (window histogram &key min max (numknots 3)  (clear t)
  925.                (thickness *histogram-thickness*) (color *histogram-color*))
  926.   "  Plot histogram as a spline on the given window.  Returns as multiple values the
  927.  histogram array, its minimum, and its maximum element values."
  928.   (let ((sequence (hist:histogram-array histogram)))
  929.     (let ((max (max (if max max 0) (reduce #'max sequence)))
  930.       (min (min (if min min 0) (reduce #'min sequence)))
  931.       (size (length sequence)))
  932.       (when clear (send window :clear))
  933.       (send window :set-image-window 0 (min 0 min) size (- max min))
  934.       (dotimes (i size)
  935.     (setf (aref *spline-xarr* i) (+ 0.5 i))
  936.     (setf (aref *spline-yarr* i) (- max (aref sequence i))))
  937.       (send window :display-spline *spline-xarr* *spline-yarr* numknots  :color color :thickness thickness)
  938.       (values sequence min max))))
  939.  
  940. ;;;;****************************************************************
  941. ;;;; MISCELLANEOUS TESTING ROUTINES
  942.  
  943. #|
  944. (defun special-test-histogram-epipolar-candidates (w grid low1 low2 high1 high2 numrows x1 y1 x2 y2
  945.         &optional (deltatheta .1) (relative-length-thresh .25))
  946.   (multiple-value-bind (lowline epi2line highline epi1line)
  947.       (clip-to-quad-init low1 low2 high2 high1)
  948. ;    (setf *lowline* lowline *epi2line* epi2line *highline* highline *epi1line* epi1line)
  949.     (let* ((vp (la:unit-normal lowline highline))
  950.        (epi (la:unit-normal epi1line epi2line))
  951.        (vp-epi-line (la:unit-normal vp epi))
  952.        (cr-reference-line (cr-reference-line lowline epi2line highline epi1line))
  953.        (1overcamz (cr-reference-value cr-reference-line vp-epi-line :reciprocal? t))
  954.        (lowz (cr-reference-value cr-reference-line lowline))
  955.        (highz (cr-reference-value cr-reference-line highline))
  956.        (cos-theta-thresh (cos (abs deltatheta)))
  957.        (candidates nil) (line nil) (trueline nil) (relative-len nil))
  958.       (setf candidates (coarse-get-lines-in-quadrilateral grid low1 low2 high2 high1))
  959.       (epipolar-display w low1 low2 high1 high2 numrows :zoom t)
  960.       (let ((pt1 (line-intersection-pt cr-reference-line lowline))
  961.         (pt2 (line-intersection-pt cr-reference-line highline)))
  962.     (display-line w (car pt1) (cadr pt1) (car pt2) (cadr pt2) numrows :color w:green)
  963.     (send w :display-point (car pt2) (- numrows (cadr pt2)) :radius 2 :color w:green))
  964.       (display-line-tokenset w candidates)
  965.       (multiple-value-bind (x1 y1 x2 y2 ok?)
  966.       (clip-to-quad  x1 y1 x2 y2 lowline epi2line highline epi1line)
  967.     (when ok?
  968.       (let ((pt1 (list x1 y1))
  969.         (pt2 (list x2 y2))
  970.         (midpt (list (/ (+ x1 x2) 2.0) (/ (+ y1 y2) 2.0))))
  971.         (setf line (endpoints-to-line pt1 pt2))
  972.         (setf trueline (pt-vec-to-line midpt vp))
  973.         (when (< cos-theta-thresh (abs (la:dot-product (firstn 2 line) (firstn 2 trueline))))
  974.           (setf relative-len 
  975.             (/ (vp:line-length x1 y1 x2 y2)
  976.                (apply #'(lambda (x1 y1 x2 y2) (vp:line-length x1 y1 x2 y2))
  977.                   (append (line-intersection-pt trueline epi1line)
  978.                       (line-intersection-pt trueline epi2line)))))
  979.           (when (>= relative-len relative-length-thresh)
  980.         (format t "Token ~s relative-len ~d~%" (list x1 y1 x2 y2) relative-len)
  981.         (format t "cross-ratios ~d ~d~%"
  982.             (height-cross-ratio
  983.               1overcamz lowz highz
  984.               (cr-reference-value cr-reference-line (pt-vec-to-line pt1 vp)))
  985.             (height-cross-ratio
  986.               1overcamz lowz highz
  987.               (cr-reference-value cr-reference-line (pt-vec-to-line pt2 vp))))
  988.         (display-line w x1 y1 x2 y2 numrows :color w:blue)
  989.         ()))))))))
  990.  
  991. (defun special-test-epipolar-search-area (w dlt1-inverse dlt2 x1 y1 x2 y2 testx1 testy1 testx2 testy2
  992.                       grid numrows &optional
  993.                       (deltatheta .1) (relative-length-thresh .25))
  994.   (multiple-value-bind (low1 low2 high1 high2)
  995.       (values-list (epipolar-search-area dlt1-inverse dlt2 x1 y1 x2 y2))
  996.     (epipolar-display w low1 low2 high1 high2 numrows)
  997.     (yes-or-no-p "continue")
  998.     (special-test-histogram-epipolar-candidates 
  999.       w grid low1 low2 high1 high2 numrows 
  1000.       testx1 testy1 testx2 testy2
  1001.       deltatheta relative-length-thresh)))
  1002.  
  1003. (defun special-test-all-epipolar-search-areas (tx1 ty1 tz1 tx2 ty2 tz2)
  1004.   (send w :restore-display 'j3lines)
  1005.   (multiple-value-bind (x1 y1) (values-list (dlt-3d-to-2d *j3dlt* tx1 ty1 tz1))
  1006.     (multiple-value-bind (x2 y2) (values-list (dlt-3d-to-2d *j3dlt* tx2 ty2 tz2))
  1007.       (display-line w x1 y1 x2 y2 *j3numrows* :color w:red)
  1008.       (mapcar
  1009.     #'(lambda (tks displayname dlt grid numrows)
  1010.         (let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
  1011.           (numcols (isr2:value (isr2:handle (list tks 'numcols))))
  1012.           (label (isr2:value (isr2:handle (list tks 'label)))))
  1013.           (set-label w2 label)
  1014.           (send w2 :set-image-window 0 0 numcols numrows))
  1015.         (send w2 :restore-display displayname)
  1016.         (let ((pt1 (dlt-3d-to-2d dlt tx1 ty1 tz1))
  1017.           (pt2 (dlt-3d-to-2d dlt tx2 ty2 tz2)))
  1018.           (special-test-epipolar-search-area 
  1019.         w2 *j3inverse* dlt x1 y1 x2 y2
  1020.         (car pt1) (cadr pt1) (car pt2) (cadr pt2)                            
  1021.         grid numrows .1 .1))
  1022.         (yes-or-no-p "next image"))
  1023.     (list 'j1-data-lines 'j2-data-lines 'j4-data-lines 
  1024.           'j5-data-lines 'j6-data-lines 'j7-data-lines 'j8-data-lines)
  1025.     (list 'j1lines 'j2lines 'j4lines 'j5lines 'j6lines 'j7lines 'j8lines)
  1026.     (list *j1dlt* *j2dlt* *j4dlt* *j5dlt* *j6dlt* *j7dlt* *j8dlt*) 
  1027.     (list *j1grid* *j2grid* *j4grid* *j5grid* *j6grid* *j7grid* *j8grid*)
  1028.     (list *j1numrows* *j2numrows* *j4numrows* *j5numrows* *j6numrows* *j7numrows* *j8numrows*)))))
  1029. |#
  1030.  
  1031. ;;;================================================================================================
  1032.  
  1033. #|
  1034. (defun project-polygon (w dlt numrows color p1 p2 p3 p4 )
  1035.   (let ((a1 (dlt-3d-to-2d dlt (car p1) (cadr p1) (caddr p1)))
  1036.     (a2 (dlt-3d-to-2d dlt (car p2) (cadr p2) (caddr p2)))
  1037.     (a3 (dlt-3d-to-2d dlt (car p3) (cadr p3) (caddr p3)))
  1038.     (a4 (dlt-3d-to-2d dlt (car p4) (cadr p4) (caddr p4))))
  1039.     (display-line w (car a1) (cadr a1) (car a2) (cadr a2) numrows :color color )
  1040.     (display-line w (car a2) (cadr a2) (car a3) (cadr a3) numrows :color color )
  1041.     (display-line w (car a3) (cadr a3) (car a4) (cadr a4) numrows :color color )
  1042.     (display-line w (car a4) (cadr a4) (car a1) (cadr a1) numrows :color color )))
  1043.  
  1044.  
  1045. (setf p1
  1046.       '((14.460031  26.344957  -0.75287825)
  1047.     (14.324111  16.400723  -0.75287825)
  1048.     (4.433068  16.535915  -0.75287825)
  1049.     (4.568989  26.48015  -0.75287825)))
  1050.  
  1051. (setf p2
  1052.       '((17.193123  13.460842  -0.079968385)
  1053.     (17.193123  0.012386719  -0.079968385)
  1054.     (-0.071631595  0.012386719  -0.079968385)
  1055.     (-0.071631595  13.460842  -0.079968385)))
  1056.  
  1057. (defvar *m1* (make-array '(3 4) :initial-contents
  1058.   '((5.701046  23.519785  -1.347012  38.638567)
  1059.     (-3.078465  2.351684  23.871563  529.155831)
  1060.     (-0.003835  0.001376  -0.000270  1.0))))
  1061.  
  1062. (defvar *m2* (make-array '(3 4) :initial-contents
  1063.   '((-1.518658  23.442719  -0.512469  58.146075)
  1064.     (-2.813365  0.551124  23.268698  555.899317)
  1065.     (-0.003952  0.000177  -0.000135  1.0))))
  1066.  
  1067. (defvar *m3* (make-array '(3 4) :initial-contents
  1068.   '((-14.561851  16.780197  -0.572034  256.198825)
  1069.     (-3.085134  -1.649948  21.897297  582.343148)
  1070.     (-0.003069  -0.002128  -0.000262  1.0))))
  1071.  
  1072. (defvar *m4* (make-array '(3 4) :initial-contents
  1073.   '((-21.566046  -2.063989  -0.517090  750.542381)
  1074.     (-0.432663  -3.013118  21.406709  590.406298)
  1075.     (-0.000049  -0.003645  -0.000193  1.0))))
  1076.  
  1077. (defvar *m5* (make-array '(3 4) :initial-contents
  1078.   '((-11.110956  -20.023827  -0.556989  1184.024421)
  1079.     (2.443573  -2.234966  22.614049  585.633526)
  1080.     (0.003146  -0.002224  -0.000220  1.0))))
  1081.  
  1082. (defvar *m6* (make-array '(3 4) :initial-contents
  1083.   '((6.500043  -23.873375  -0.610927  1243.099897)
  1084.     (3.658103  0.126230  24.421587  563.681390)
  1085.     (0.004111  0.000651  -0.000252  1.0))))
  1086.  
  1087. (defvar *m7* (make-array '(3 4) :initial-contents
  1088.   '((26.021084  4.941464  -0.575666  467.823417)
  1089.     (0.229142  3.205019  26.236815  544.716835)
  1090.     (-0.000349  0.004447  -0.000148  1.0))))
  1091.  
  1092. (defvar *m8* (make-array '(3 4) :initial-contents
  1093.   '((18.152642  17.996889  -0.610551  153.248636)
  1094.     (-1.858501  3.078040  25.256988  536.382476)
  1095.     (-0.002679  0.003366  -0.000228  1.0))))
  1096.  
  1097.  
  1098. (defun project-polygon-vertices (dlt point-list)
  1099.   (mapcar #'(lambda (p) (dlt-3d-to-2d dlt (car p) (cadr p) (caddr p)))
  1100.       point-list))
  1101.  
  1102. (defun project-polygons-postscript-output (stream dlt polygon-list)
  1103.   (terpri stream)
  1104.   (dolist (poly polygon-list)
  1105.     (format stream "%%polygon index: ~d~%newpath~%" (car poly))
  1106.     (let ((points (project-polygon-vertices dlt (cadr poly))))
  1107.       (format stream "~d ~d moveto~%" (car (car points)) (cadr (car points)))
  1108.       (dolist (pt (cdr points))
  1109.     (format stream "~d ~d lineto~%"  (car pt) (cadr pt)))
  1110.       (format stream "closepath stroke~%"))))
  1111.  
  1112. (defun project-2d-polygons-postscript-output (stream polygon-list &aux (index 0))
  1113.   (terpri stream)
  1114.   (dolist (poly polygon-list)
  1115.     (format stream "%%polygon index: ~d~%newpath~%" (incf index))
  1116.     (let ((points poly))
  1117.       (format stream "~d ~d moveto~%" (car (car points)) (cadr (car points)))
  1118.       (dolist (pt (cdr points))
  1119.     (format stream "~d ~d lineto~%"  (car pt) (cadr pt)))
  1120.       (format stream "closepath stroke~%"))))
  1121.  
  1122. (defun read-poly (stream)
  1123.   (let ((result nil))
  1124.     (dotimes (i (read stream) (reverse result))
  1125.       (push (list (read stream) (read stream) (read stream)) result))))
  1126.  
  1127. (defun read-xg-format-polygons (filename)
  1128.   (let ((result nil))
  1129.     (with-open-file (file filename :direction :input)
  1130.       (let ((numpolygons (read file)))
  1131.     (dotimes (i numpolygons (cdr (reverse result)))
  1132.       (push (list (read file) (read-poly file)) result))))))
  1133.  
  1134.  
  1135.  
  1136.  
  1137. |#
  1138.  
  1139. #|
  1140.  
  1141.  
  1142. (defvar *line-matches* nil)
  1143.  
  1144. (defun j1-test-all-epipolar-search-areas (linelist &key (endpoint-slop 1.0) (delta-theta .1)
  1145.                       (prompt t) (prompt-first nil))
  1146.   (let ((w *display1*)
  1147.     (w2 *display2*)
  1148.     (first t))
  1149.     (setf *line-matches* nil)
  1150.     (clear-height-histogram *accum-histogram*)
  1151.     (send *plot1* :clear)
  1152.     (send *plot2* :clear)
  1153.     (dolist (line linelist)
  1154.       (push line *line-matches*)
  1155.       (multiple-value-bind (x1 y1 x2 y2) (values-list line)
  1156.     (send w :restore-display 'j1lines)
  1157.     (display-line w x1 y1 x2 y2 *j1numrows* :color *search-color* :thickness (1+ *search-thickness*))
  1158.     (mapcar
  1159.       #'(lambda (tks displayname dlt grid numrows)
  1160.           (let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
  1161.             (numcols (isr2:value (isr2:handle (list tks 'numcols))))
  1162.             (label (isr2:value (isr2:handle (list tks 'label)))))
  1163.         (set-label w2 label)
  1164.         (send w2 :set-image-window 0 0 numcols numrows))
  1165.           (when prompt
  1166.         (send w2 :restore-display displayname))
  1167.           (clear-height-histogram *current-histogram*)
  1168.           (test-epipolar-search-area w2 *j1inverse* dlt x1 y1 x2 y2 grid numrows
  1169.                      :deltatheta delta-theta :relative-length-thresh .1
  1170.                      :endpoint-slop endpoint-slop :prompt prompt)
  1171.           (plot-histogram *plot2* *current-histogram* :max 1)
  1172.           (add-to-height-histogram *accum-histogram* *current-histogram*)
  1173.           (plot-histogram *plot1* *accum-histogram* :max 3)
  1174.           (when (and first prompt-first)
  1175.         (yes-or-no-p "continue"))
  1176.           (setf first nil)        
  1177.           (when prompt
  1178.         (do ((done (yes-or-no-p "next image") (yes-or-no-p "next image")))
  1179.             (done t)
  1180.           (print "enter line coordinate list (x1 y1 x2 y2)")
  1181.           (let ((testline (eval (read))))
  1182.             (test-epipolar-search-area w2 *j1inverse* dlt x1 y1 x2 y2 grid numrows
  1183.                            :deltatheta delta-theta
  1184.                            :relative-length-thresh .1 :testline testline
  1185.                            :endpoint-slop endpoint-slop)))))
  1186.       (list 'j3-data-lines 'j2-data-lines 'j4-data-lines 
  1187.         'j5-data-lines 'j6-data-lines 'j7-data-lines 'j8-data-lines)
  1188.       (list 'j3lines 'j2lines 'j4lines 'j5lines 'j6lines 'j7lines 'j8lines)
  1189.       (list *j3dlt* *j2dlt* *j4dlt* *j5dlt* *j6dlt* *j7dlt* *j8dlt*) 
  1190.       (list *j3grid* *j2grid* *j4grid* *j5grid* *j6grid* *j7grid* *j8grid*)
  1191.       (list *j3numrows* *j2numrows* *j4numrows* *j5numrows* *j6numrows* *j7numrows* *j8numrows*))))
  1192.     *accum-histogram*))
  1193.  
  1194. (defun histogram-epipolar-candidates (w grid low1 low2 high1 high2 numrows
  1195.         &key (deltatheta .1) (relative-length-thresh .25) (histogram *current-histogram*)
  1196.         (endpoint-slop 1) (prompt t))
  1197.   (multiple-value-bind (lowline epi2line highline epi1line)
  1198.       (clip-to-quad-init low1 low2 high2 high1)
  1199. ;    (setf *lowline* lowline *epi2line* epi2line *highline* highline *epi1line* epi1line)
  1200.     (let* ((vp (la:unit-normal lowline highline))
  1201.        (epi (la:unit-normal epi1line epi2line))
  1202.        (vp-epi-line (la:unit-normal vp epi))
  1203.        (cr-reference-line (cr-reference-line lowline epi2line highline epi1line))
  1204.        (1overcamz (cr-reference-value cr-reference-line vp-epi-line :reciprocal? t))
  1205.        (lowz (cr-reference-value cr-reference-line lowline))
  1206.        (highz (cr-reference-value cr-reference-line highline))
  1207.        (cos-theta-thresh (cos (abs deltatheta)))
  1208.        (candidates nil) (line nil) (trueline nil) (relative-len nil))
  1209.       (setf candidates (coarse-get-lines-in-quadrilateral grid low1 low2 high2 high1))
  1210.       (epipolar-display w low1 low2 high1 high2 numrows :zoom t)
  1211. ;      (let ((pt1 (line-intersection-pt cr-reference-line lowline))
  1212. ;        (pt2 (line-intersection-pt cr-reference-line highline)))
  1213. ;    (display-line w (car pt1) (cadr pt1) (car pt2) (cadr pt2) numrows :color w:green)
  1214. ;    (send w :display-point (car pt2) (- numrows (cadr pt2)) :radius 2 :color w:green))
  1215.       (display-line-tokenset w candidates :color *line-color* :thickness *line-thickness*)
  1216.       (epipolar-display w low1 low2 high1 high2 numrows :zoom nil)
  1217.       (isr2:for-every-token (tok candidates (x1 y1 x2 y2))
  1218.     (multiple-value-bind (x1 y1 x2 y2 ok?)
  1219.         (clip-to-quad (isr2:value x1) (isr2:value y1) (isr2:value x2) (isr2:value y2)
  1220.               lowline epi2line highline epi1line)
  1221.       (when ok?
  1222.         (let ((pt1 (list x1 y1))
  1223.           (pt2 (list x2 y2))
  1224.           (midpt (list (/ (+ x1 x2) 2.0) (/ (+ y1 y2) 2.0))))
  1225.           (setf line (endpoints-to-line pt1 pt2))
  1226.           (setf trueline (pt-vec-to-line midpt vp))
  1227.           (when (< cos-theta-thresh (abs (la:dot-product (firstn 2 line) (firstn 2 trueline))))
  1228.         (setf relative-len 
  1229.               (/ (vp:line-length x1 y1 x2 y2)
  1230.              (apply #'(lambda (x1 y1 x2 y2) (vp:line-length x1 y1 x2 y2))
  1231.                 (append (line-intersection-pt trueline epi1line)
  1232.                     (line-intersection-pt trueline epi2line)))))
  1233.         (when (>= relative-len relative-length-thresh)
  1234.           (multiple-value-setq (pt1 pt2) (sloppy-points pt1 pt2 trueline endpoint-slop))
  1235.           (let ((zvalue1 (cr-reference-value cr-reference-line (pt-vec-to-line pt1 vp)))
  1236.             (zvalue2 (cr-reference-value cr-reference-line (pt-vec-to-line pt2 vp))))
  1237.             (multiple-value-bind (lowheight highheight)
  1238.             (vote-for-height-range 
  1239.               (height-cross-ratio 1overcamz lowz highz zvalue1)
  1240.               (height-cross-ratio 1overcamz lowz highz zvalue2)
  1241.               *1overcamz* *lowz* *highz*
  1242.               :weight relative-len
  1243.               :histogram histogram)
  1244.               (push (isr2:copy-handle tok) *line-matches*)
  1245.               (when prompt
  1246.             (format t "Token ~s votes ~d for height range (~d,~d)~%" 
  1247.                 tok relative-len lowheight highheight)))
  1248. ;            (display-line w (car pt1) (cadr pt1) (car pt2) (cadr pt2) numrows
  1249. ;                  :color w:red :thickness 3)
  1250.             (display-line w x1 y1 x2 y2 numrows :color *search-color* :thickness *search-thickness*)
  1251.             ()))))))))))
  1252.  
  1253.  
  1254. (defun get-J-image-number (tok)
  1255.   (let ((str (format nil "~a" tok)))
  1256.     (let ((ind (+ 1 (position "J" str :test #'string-equal))))
  1257.       (read-from-string (subseq str ind (+ 1 ind))))))
  1258.  
  1259.  
  1260. (defun save-matches-to-file (filename)
  1261.   (with-open-file (file filename :direction :output)
  1262.     (format file "matched line data from L-shaped building~%")
  1263.     (dolist (item (reverse *line-matches*) t)
  1264.       (if (listp item)
  1265.       (progn
  1266.         (format file "===============================~%")
  1267.         (format file "1 ~d ~d ~d ~d~%" 
  1268.             (coerce (car item) 'single-float)
  1269.             (coerce (cadr item) 'single-float)
  1270.             (coerce (caddr item) 'single-float)
  1271.             (coerce (fourth item) 'single-float)))
  1272.       (format file "~d ~d ~d ~d ~d~%"
  1273.           (get-J-image-number item)
  1274.           (isr2:value (list item 'x1))
  1275.           (isr2:value (list item 'y1))
  1276.           (isr2:value (list item 'x2))
  1277.           (isr2:value (list item 'y2)))))))
  1278. |#
  1279.  
  1280. (defvar *matching-lines* nil "used to collect matching lines")
  1281.  
  1282. (defun collect-match-candidates  (w grid line-index low1 low2 high1 high2 numrows
  1283.         &key (deltatheta .1) (relative-length-thresh .25))
  1284.   (multiple-value-bind (lowline epi2line highline epi1line)
  1285.       (clip-to-quad-init low1 low2 high2 high1)
  1286.     (let* ((vp (la:unit-normal lowline highline))
  1287.        (cos-theta-thresh (cos (abs deltatheta)))
  1288.        (candidates nil) (line nil) (trueline nil) (relative-len nil))
  1289.       (setf candidates (coarse-get-lines-in-quadrilateral grid low1 low2 high2 high1))
  1290.       (epipolar-display w low1 low2 high1 high2 numrows :zoom t)
  1291.       (display-line-tokenset w candidates :color *line-color* :thickness *line-thickness*)
  1292.       (epipolar-display w low1 low2 high1 high2 numrows :zoom nil)
  1293.       (isr2:for-every-token (tok candidates (x1 y1 x2 y2))
  1294.     (multiple-value-bind (x1 y1 x2 y2 ok?)
  1295.         (clip-to-quad (isr2:value x1) (isr2:value y1) (isr2:value x2) (isr2:value y2)
  1296.               lowline epi2line highline epi1line)
  1297.       (when ok?
  1298.         (let ((pt1 (list x1 y1))
  1299.           (pt2 (list x2 y2))
  1300.           (midpt (list (/ (+ x1 x2) 2.0) (/ (+ y1 y2) 2.0))))
  1301.  
  1302.           (setf line (endpoints-to-line pt1 pt2))
  1303.           (setf trueline (pt-vec-to-line midpt vp))
  1304.           (when (< cos-theta-thresh (abs (la:dot-product (firstn 2 line) (firstn 2 trueline))))
  1305.         (setf relative-len 
  1306.               (/ (vp:line-length x1 y1 x2 y2)
  1307.              (apply #'(lambda (x1 y1 x2 y2) (vp:line-length x1 y1 x2 y2))
  1308.                 (append (line-intersection-pt trueline epi1line)
  1309.                     (line-intersection-pt trueline epi2line)))))
  1310.         (when (>= relative-len relative-length-thresh)
  1311.           (push (list line-index (isr2:copy-handle tok)) 
  1312.             *matching-lines* )
  1313.           (display-line w x1 y1 x2 y2 numrows :color *search-color* :thickness *search-thickness*)
  1314.           ())))))))))
  1315.  
  1316. (defun epipolar-collection-area (dlt1-inverse dlt2 x1 y1 x2 y2 z-range endpoint-slop)
  1317.   (let* ((low1 (apply #'dlt-3d-to-2d dlt2 (backproject-point dlt1-inverse x1 y1 (car z-range))))
  1318.      (low2 (apply #'dlt-3d-to-2d dlt2 (backproject-point dlt1-inverse x2 y2 (car z-range))))
  1319.      (high1 (apply #'dlt-3d-to-2d dlt2 (backproject-point dlt1-inverse x1 y1 (cadr z-range))))
  1320.      (high2 (apply #'dlt-3d-to-2d dlt2 (backproject-point dlt1-inverse x2 y2 (cadr z-range))))
  1321.      (lowline (endpoints-to-line low1 low2))
  1322.      (highline (endpoints-to-line high1 high2))
  1323.      (slop (if (minusp (la:dot-product lowline (pt-to-nvec (car high1) (cadr high1))))
  1324.            (- (abs endpoint-slop))
  1325.            (abs endpoint-slop)))
  1326.      (sloplowx (* (car lowline) (- slop)))
  1327.      (sloplowy (* (cadr lowline) (- slop)))
  1328.      (slophighx (* (car highline) slop))
  1329.      (slophighy (* (cadr highline) slop)))
  1330.     (list
  1331.       (list (+ (car low1) sloplowx) (+ (cadr low1) sloplowy))
  1332.       (list (+ (car low2) sloplowx) (+ (cadr low2) sloplowy))
  1333.       (list (+ (car high1) slophighx) (+ (cadr high1) slophighy))
  1334.       (list (+ (car high2) slophighx) (+ (cadr high2) slophighy))
  1335.       low1 low2 high1 high2)))
  1336.  
  1337. (defun collect-lines-from-epipolar-area (w dlt1-inverse dlt2 line-index x1 y1 x2 y2 z-range grid numrows
  1338.                     &key (deltatheta .1) (relative-length-thresh .25)
  1339.                     (endpoint-slop 1) (prompt t))
  1340.   (multiple-value-bind (low1 low2 high1 high2 l1 l2 h1 h2)
  1341.       (values-list (epipolar-collection-area dlt1-inverse dlt2 x1 y1 x2 y2 z-range endpoint-slop))
  1342.     (when prompt
  1343.       (epipolar-display w l1 l2 h1 h2 numrows :zoom t :color w:red :thickness 1)
  1344.       (epipolar-display w low1 low2 high1 high2 numrows)
  1345.       (yes-or-no-p "continue"))
  1346.     (collect-match-candidates 
  1347.       w grid line-index low1 low2 high1 high2 numrows
  1348.       :deltatheta deltatheta 
  1349.       :relative-length-thresh relative-length-thresh)))
  1350.  
  1351. (defun collect-matching-lines (linelist z-range &key (endpoint-slop 1.0)(delta-theta .1)
  1352.                    (prompt t) (prompt-first nil))
  1353.   (let ((w *display1*)
  1354.     (w2 *display2*)
  1355.     (first t)
  1356.     (line-index 0))
  1357.     (setf *matching-lines* nil)
  1358.     (send *plot2* :clear)
  1359.     (dolist (line linelist)
  1360.       (incf line-index)
  1361.       (multiple-value-bind (x1 y1 x2 y2) (values-list line)
  1362.     (send w :restore-display 'j3lines)
  1363.     (display-line w x1 y1 x2 y2 *j3numrows* :color *search-color* :thickness (1+ *search-thickness*))
  1364.     (mapcar
  1365.       #'(lambda (tks displayname dlt grid numrows)
  1366.           (let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
  1367.             (numcols (isr2:value (isr2:handle (list tks 'numcols))))
  1368.             (label (isr2:value (isr2:handle (list tks 'label)))))
  1369.         (set-label w2 label)
  1370.         (send w2 :set-image-window 0 0 numcols numrows))
  1371.           (when prompt
  1372.         (send w2 :restore-display displayname))
  1373.           (collect-lines-from-epipolar-area
  1374.         w2 *j3inverse* dlt 
  1375.         line-index x1 y1 x2 y2 z-range
  1376.         grid numrows
  1377.         :deltatheta delta-theta :relative-length-thresh .1
  1378.         :endpoint-slop endpoint-slop :prompt prompt)
  1379.           (when (and first prompt-first)
  1380.         (yes-or-no-p "continue"))
  1381.           (setf first nil)
  1382.           (when prompt (yes-or-no-p "next image")))
  1383.       (list 'j1-data-lines 'j2-data-lines 'j3-data-lines 'j4-data-lines 
  1384.         'j5-data-lines 'j6-data-lines 'j7-data-lines 'j8-data-lines)
  1385.       (list 'j1lines 'j2lines 'j3lines 'j4lines 'j5lines 'j6lines 'j7lines 'j8lines)
  1386.       (list *j1dlt* *j2dlt* *j3dlt* *j4dlt* *j5dlt* *j6dlt* *j7dlt* *j8dlt*) 
  1387.       (list *j1grid* *j2grid* *j3grid* *j4grid* *j5grid* *j6grid* *j7grid* *j8grid*)
  1388.       (list *j1numrows* *j2numrows* *j3numrows* *j4numrows* 
  1389.         *j5numrows* *j6numrows* *j7numrows* *j8numrows*))))
  1390.     *matching-lines*))
  1391.  
  1392. (defun j8-collect-matching-lines (linelist z-range &key (endpoint-slop 1.0)(delta-theta .1)
  1393.                    (prompt t) (prompt-first nil))
  1394.   (let ((w *display1*)
  1395.     (w2 *display2*)
  1396.     (first t)
  1397.     (line-index 0))
  1398.     (setf *matching-lines* nil)
  1399.     (send *plot2* :clear)
  1400.     (dolist (line linelist)
  1401.       (incf line-index)
  1402.       (multiple-value-bind (x1 y1 x2 y2) (values-list line)
  1403.     (send w :restore-display 'j8lines)
  1404.     (display-line w x1 y1 x2 y2 *j8numrows* :color *search-color* :thickness (1+ *search-thickness*))
  1405.     (mapcar
  1406.       #'(lambda (tks displayname dlt grid numrows)
  1407.           (let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
  1408.             (numcols (isr2:value (isr2:handle (list tks 'numcols))))
  1409.             (label (isr2:value (isr2:handle (list tks 'label)))))
  1410.         (set-label w2 label)
  1411.         (send w2 :set-image-window 0 0 numcols numrows))
  1412.           (when prompt
  1413.         (send w2 :restore-display displayname))
  1414.           (collect-lines-from-epipolar-area
  1415.         w2 *j8inverse* dlt 
  1416.         line-index x1 y1 x2 y2 z-range
  1417.         grid numrows
  1418.         :deltatheta delta-theta :relative-length-thresh .1
  1419.         :endpoint-slop endpoint-slop :prompt prompt)
  1420.           (when (and first prompt-first)
  1421.         (yes-or-no-p "continue"))
  1422.           (setf first nil)
  1423.           (when prompt (yes-or-no-p "next image")))
  1424.       (list 'j1-data-lines 'j2-data-lines 'j3-data-lines 'j4-data-lines 
  1425.         'j5-data-lines 'j6-data-lines 'j7-data-lines 'j8-data-lines)
  1426.       (list 'j1lines 'j2lines 'j3lines 'j4lines 'j5lines 'j6lines 'j7lines 'j8lines)
  1427.       (list *j1dlt* *j2dlt* *j3dlt* *j4dlt* *j5dlt* *j6dlt* *j7dlt* *j8dlt*) 
  1428.       (list *j1grid* *j2grid* *j3grid* *j4grid* *j5grid* *j6grid* *j7grid* *j8grid*)
  1429.       (list *j1numrows* *j2numrows* *j3numrows* *j4numrows* 
  1430.         *j5numrows* *j6numrows* *j7numrows* *j8numrows*))))
  1431.     *matching-lines*))
  1432.  
  1433. (defun j6-collect-matching-lines (linelist z-range &key (endpoint-slop 1.0)(delta-theta .1)
  1434.                    (prompt t) (prompt-first nil))
  1435.   (let ((w *display1*)
  1436.     (w2 *display2*)
  1437.     (first t)
  1438.     (line-index 0))
  1439.     (setf *matching-lines* nil)
  1440.     (send *plot2* :clear)
  1441.     (dolist (line linelist)
  1442.       (incf line-index)
  1443.       (multiple-value-bind (x1 y1 x2 y2) (values-list line)
  1444.     (send w :restore-display 'j6lines)
  1445.     (display-line w x1 y1 x2 y2 *j6numrows* :color *search-color* :thickness (1+ *search-thickness*))
  1446.     (mapcar
  1447.       #'(lambda (tks displayname dlt grid numrows)
  1448.           (let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
  1449.             (numcols (isr2:value (isr2:handle (list tks 'numcols))))
  1450.             (label (isr2:value (isr2:handle (list tks 'label)))))
  1451.         (set-label w2 label)
  1452.         (send w2 :set-image-window 0 0 numcols numrows))
  1453.           (when prompt
  1454.         (send w2 :restore-display displayname))
  1455.           (collect-lines-from-epipolar-area
  1456.         w2 *j6inverse* dlt 
  1457.         line-index x1 y1 x2 y2 z-range
  1458.         grid numrows
  1459.         :deltatheta delta-theta :relative-length-thresh .1
  1460.         :endpoint-slop endpoint-slop :prompt prompt)
  1461.           (when (and first prompt-first)
  1462.         (yes-or-no-p "continue"))
  1463.           (setf first nil)
  1464.           (when prompt (yes-or-no-p "next image")))
  1465.       (list 'j1-data-lines 'j2-data-lines 'j3-data-lines 'j4-data-lines 
  1466.         'j5-data-lines 'j6-data-lines 'j7-data-lines 'j8-data-lines)
  1467.       (list 'j1lines 'j2lines 'j3lines 'j4lines 'j5lines 'j6lines 'j7lines 'j8lines)
  1468.       (list *j1dlt* *j2dlt* *j3dlt* *j4dlt* *j5dlt* *j6dlt* *j7dlt* *j8dlt*) 
  1469.       (list *j1grid* *j2grid* *j3grid* *j4grid* *j5grid* *j6grid* *j7grid* *j8grid*)
  1470.       (list *j1numrows* *j2numrows* *j3numrows* *j4numrows* 
  1471.         *j5numrows* *j6numrows* *j7numrows* *j8numrows*))))
  1472.     *matching-lines*))
  1473.  
  1474. (defun j8-test-all-epipolar-search-areas (linelist &key (endpoint-slop 1.0)(delta-theta .1)
  1475.                        (prompt t) (prompt-first nil))
  1476.   (let ((w *display1*)
  1477.     (w2 *display2*)
  1478.     (first t))
  1479.     (clear-height-histogram *accum-histogram*)
  1480.     (send *plot1* :clear)
  1481.     (send *plot2* :clear)
  1482.     (dolist (line linelist)
  1483.       (multiple-value-bind (x1 y1 x2 y2) (values-list line)
  1484.     (send w :restore-display 'j8lines)
  1485.     (display-line w x1 y1 x2 y2 *j8numrows* :color *search-color* :thickness (1+ *search-thickness*))
  1486.     (mapcar
  1487.       #'(lambda (tks displayname dlt grid numrows)
  1488.           (let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
  1489.             (numcols (isr2:value (isr2:handle (list tks 'numcols))))
  1490.             (label (isr2:value (isr2:handle (list tks 'label)))))
  1491.         (set-label w2 label)
  1492.         (send w2 :set-image-window 0 0 numcols numrows))
  1493.           (when prompt
  1494.         (send w2 :restore-display displayname))
  1495.           (clear-height-histogram *current-histogram*)
  1496.           (test-epipolar-search-area w2 *j8inverse* dlt x1 y1 x2 y2 grid numrows
  1497.                      :deltatheta delta-theta :relative-length-thresh .1
  1498.                      :endpoint-slop endpoint-slop :prompt prompt)
  1499.           (plot-histogram *plot2* *current-histogram* :max 1)
  1500.           (add-to-height-histogram *accum-histogram* *current-histogram*)
  1501.           (plot-histogram *plot1* *accum-histogram* :max 3)
  1502.           (when (and first prompt-first)
  1503.         (yes-or-no-p "continue"))
  1504.           (setf first nil)        
  1505.           (when prompt
  1506.         (do ((done (yes-or-no-p "next image") (yes-or-no-p "next image")))
  1507.             (done t)
  1508.           (print "enter line coordinate list (x1 y1 x2 y2)")
  1509.           (let ((testline (eval (read))))
  1510.             (test-epipolar-search-area w2 *j8inverse* dlt x1 y1 x2 y2 grid numrows
  1511.                            :deltatheta delta-theta :relative-length-thresh .1 
  1512.                            :testline testline
  1513.                            :endpoint-slop endpoint-slop)))))
  1514.       (list 'j1-data-lines 'j2-data-lines 'j3-data-lines 'j4-data-lines 
  1515.         'j5-data-lines 'j6-data-lines 'j7-data-lines )
  1516.       (list 'j1lines 'j2lines 'j3lines 'j4lines 'j5lines 'j6lines 'j7lines )
  1517.       (list *j1dlt* *j2dlt* *j3dlt* *j4dlt* *j5dlt* *j6dlt* *j7dlt*) 
  1518.       (list *j1grid* *j2grid* *j3grid* *j4grid* *j5grid* *j6grid* *j7grid*)
  1519.       (list *j1numrows* *j2numrows* *j3numrows* *j4numrows* *j5numrows* *j6numrows* *j7numrows*))))
  1520.     *accum-histogram*))
  1521.  
  1522. (defun j6-test-all-epipolar-search-areas (linelist &key (endpoint-slop 1.0)(delta-theta .1)
  1523.                        (prompt t) (prompt-first nil))
  1524.   (let ((w *display1*)
  1525.     (w2 *display2*)
  1526.     (first t))
  1527.     (clear-height-histogram *accum-histogram*)
  1528.     (send *plot1* :clear)
  1529.     (send *plot2* :clear)
  1530.     (dolist (line linelist)
  1531.       (multiple-value-bind (x1 y1 x2 y2) (values-list line)
  1532.     (send w :restore-display 'j6lines)
  1533.     (display-line w x1 y1 x2 y2 *j6numrows* :color *search-color* :thickness (1+ *search-thickness*))
  1534.     (mapcar
  1535.       #'(lambda (tks displayname dlt grid numrows)
  1536.           (let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
  1537.             (numcols (isr2:value (isr2:handle (list tks 'numcols))))
  1538.             (label (isr2:value (isr2:handle (list tks 'label)))))
  1539.         (set-label w2 label)
  1540.         (send w2 :set-image-window 0 0 numcols numrows))
  1541.           (when prompt
  1542.         (send w2 :restore-display displayname))
  1543.           (clear-height-histogram *current-histogram*)
  1544.           (test-epipolar-search-area w2 *j6inverse* dlt x1 y1 x2 y2 grid numrows
  1545.                      :deltatheta delta-theta :relative-length-thresh .1
  1546.                      :endpoint-slop endpoint-slop :prompt prompt)
  1547.           (plot-histogram *plot2* *current-histogram* :max 1)
  1548.           (add-to-height-histogram *accum-histogram* *current-histogram*)
  1549.           (plot-histogram *plot1* *accum-histogram* :max 3)
  1550.           (when (and first prompt-first)
  1551.         (yes-or-no-p "continue"))
  1552.           (setf first nil)        
  1553.           (when prompt
  1554.         (do ((done (yes-or-no-p "next image") (yes-or-no-p "next image")))
  1555.             (done t)
  1556.           (print "enter line coordinate list (x1 y1 x2 y2)")
  1557.           (let ((testline (eval (read))))
  1558.             (test-epipolar-search-area w2 *j6inverse* dlt x1 y1 x2 y2 grid numrows
  1559.                            :deltatheta delta-theta :relative-length-thresh .1 
  1560.                            :testline testline
  1561.                            :endpoint-slop endpoint-slop)))))
  1562.       (list 'j1-data-lines 'j2-data-lines 'j3-data-lines 'j4-data-lines 
  1563.         'j5-data-lines 'j7-data-lines 'j8-data-lines )
  1564.       (list 'j1lines 'j2lines 'j3lines 'j4lines 'j5lines 'j7lines 'j8lines )
  1565.       (list *j1dlt* *j2dlt* *j3dlt* *j4dlt* *j5dlt* *j7dlt* *j8dlt*) 
  1566.       (list *j1grid* *j2grid* *j3grid* *j4grid* *j5grid* *j7grid* *j8grid*)
  1567.       (list *j1numrows* *j2numrows* *j3numrows* *j4numrows* *j5numrows* *j7numrows* *j8numrows*))))
  1568.     *accum-histogram*))
  1569.  
  1570. (defun get-J-image-number (tok)
  1571.   (let ((str (format nil "~a" tok)))
  1572.     (let ((ind (+ 1 (position "J" str :test #'string-equal))))
  1573.       (read-from-string (subseq str ind (+ 1 ind))))))
  1574.  
  1575. (defun get-all-J-matches (image-number)
  1576.   (mapcan #'(lambda (match-pair) 
  1577.           (when (eq (get-J-image-number (cadr match-pair)) image-number)
  1578.         (list match-pair)))
  1579.  
  1580.       *matching-lines*))
  1581.  
  1582. (defvar *dlt-list* (list *j1dlt*  *j2dlt*  *j3dlt*  *j4dlt* *j5dlt*  *j6dlt*  *j7dlt*  *j8dlt*))
  1583.  
  1584. (defun save-matches-to-file (linelist filename &optional (numimages 8) &aux numlines)
  1585.   (setf numlines (length linelist))
  1586.   (with-open-file (file filename :direction :output)
  1587.     (format file "~d~%" numlines)   ;number of polygon lines
  1588.     (format file "~d~%" numlines)   ;perpendicularity constraints
  1589.     (dotimes (i numlines)
  1590.       (format file "~d ~d  0.0~%" 
  1591.           (+ 1 i)
  1592.           (if (= i (- numlines 1))
  1593.           1
  1594.           (+ 2 i))))
  1595.     (format file "~d~%" numlines)   ;horizontal line constraints
  1596.     (dotimes (i numlines)
  1597.       (format file "~d~%" (+ 1 i)))
  1598.     (format file "~d~%" numimages)    
  1599.     (dotimes (image numimages filename)   ;DLT matrix for this image
  1600.       (dotimes (r 3) 
  1601.     (dotimes (c 4)
  1602.       (unless (= (+ r c) 5)
  1603.         (format file "~f  " (aref (elt *dlt-list* image) r c))))
  1604.     (format file "~%"))
  1605.       (let ((matches (get-all-j-matches (+ image 1))))
  1606.     (format file "~d~%" (length matches))
  1607.     (dolist (match (reverse matches) nil)
  1608.       (let ((index (car match))
  1609.         (tok (cadr match)))
  1610.         (format file "~d ~d ~d ~d ~d~%"    ;write one line match
  1611.             index
  1612.             (isr2:value (list tok 'x1))
  1613.             (isr2:value (list tok 'y1))
  1614.             (isr2:value (list tok 'x2))
  1615.             (isr2:value (list tok 'y2)))))))))
  1616.  
  1617. (defun redisplay-lines (w tks displayname)
  1618.   (let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
  1619.     (numcols (isr2:value (isr2:handle (list tks 'numcols))))
  1620.     (label (isr2:value (isr2:handle (list tks 'label)))))
  1621.     (set-label w label)
  1622.     (send w :set-image-window 0 0 numcols numrows))
  1623.   (send w :restore-display displayname))
  1624.