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
/
ascender.tar.Z
/
ascender.tar
/
Epipolar
/
epipolar.lisp
< prev
next >
Wrap
Lisp/Scheme
|
1995-07-20
|
73KB
|
1,624 lines
;;; -*- Mode:Common-Lisp; Package:USER; Fonts:(MEDFNT HL12B HL12BI MEDFNT MEDFNB); Base:10 -*-
(in-package 'user)
;;;;FIRST MUST EXECUTE (load "unix:lisp;load-new-vanishing-point")
;;;; (load "unix:lisp;histogram")
;;;; (load "unix:lisp;plot")
(defvar *demo-mode* t "tells whether to display things while processing")
(defvar *lowz* -2.2)
(defvar *highz* 3.5)
(defvar *1overcamz* (/ 1.0 165.33823))
(defvar *num-z-buckets* 24 "default number of histogram buckets")
(defvar *current-histogram* nil "height histogram for this image")
(defvar *accum-histogram* nil "accumulated height histogram")
(defvar *spline-xarr* nil "array for display histograms as splines")
(defvar *spline-yarr* nil "array for display histograms as splines")
(defvar *display1* nil "display window")
(defvar *display2* nil "second display window")
(defvar *plot1* nil "plot window")
(defvar *plot2* nil "second plot window")
(defvar *no-labels* nil "if nonnil, means don't display window labels - for video purposes")
;;;;==================== COLOR AND THICKNESS SETTINGS ========================
(defvar *histogram-color* w:black)
(defvar *histogram-thickness* 3)
(defvar *epipolar-color* w:green)
(defvar *epipolar-thickness* 3)
(defvar *search-color* w:blue)
(defvar *search-thickness* 3)
(defvar *line-color* w:black)
(defvar *line-thickness* 1)
;;;============================================================================
(defvar *j1grid* nil)
(defvar *j2grid* nil)
(defvar *j3grid* nil)
(defvar *j4grid* nil)
(defvar *j5grid* nil)
(defvar *j6grid* nil)
(defvar *j7grid* nil)
(defvar *j8grid* nil)
(defvar *j1numrows* nil)
(defvar *j2numrows* nil)
(defvar *j3numrows* nil)
(defvar *j4numrows* nil)
(defvar *j5numrows* nil)
(defvar *j6numrows* nil)
(defvar *j7numrows* nil)
(defvar *j8numrows* nil)
(defvar *j1dlt* (make-array '(3 4) :initial-contents '(
(-19.960773 0.22749877 -12.903301 842.667 )
(-3.4838574 -23.273052 5.303175 1114.2806 )
(0.0015737481 -0.0012492101 -0.0026543885 1.0))))
(defvar *j2dlt* (make-array '(3 4) :initial-contents '(
(-1.7695084 -19.28334 13.439316 1076.7924 )
(23.069756 0.33550453 3.4454346 188.71094 )
(0.000654608 -0.002153894 -0.0017601326 1.0))))
(defvar *j3dlt* (make-array '(3 4) :initial-contents '(
(-23.920588 -1.4487858 -1.3064575 933.0063 )
(1.5003289 -24.09358 -0.61531067 976.0755 )
(-0.0000140998745 -0.0002232939 -0.0027317852 1.0))))
(defvar *j4dlt* (make-array '(3 4) :initial-contents '(
(0.4836445 19.066517 -13.104324 188.70502 )
(-23.331348 -0.13543749 -1.3505707 812.1729 )
(-0.00026167184 -0.0017117839 -0.003778996 1.0))))
(defvar *j5dlt* (make-array '(3 4) :initial-contents '(
(-0.5118285 23.024166 9.621094 314.9109 )
(-24.943178 0.011466533 -0.47771835 763.81146 )
(-0.000101438665 0.001958397 -0.0029221326 1.0))))
(defvar *j6dlt* (make-array '(3 4) :initial-contents '(
(2.3561907 23.421192 -5.657258 145.03972 )
(-23.42959 0.923416 -5.657257 766.7126 )
(0.0005539898 -0.00046683103 -0.0041301455 1.0))))
(defvar *j7dlt* (make-array '(3 4) :initial-contents '(
(0.14835072 23.38354 -4.2434387 197.26375 )
(-23.46 1.1095862 4.074173 835.20154 )
(-0.0012843199 -0.00017914758 -0.004245341 1.0))))
(defvar *j8dlt* (make-array '(3 4) :initial-contents '(
(0.55907404 24.959345 3.1993098 179.15173 )
(-17.57395 2.7324274 -16.89434 630.7562 )
(0.0026008417 0.0008867616 -0.0020301389 1.0))))
#| OLD SET OF DLT PARAMETERS
;(defvar *j1dlt* (make-array '(3 4) :initial-contents '(
; ( -19.8644 0.178765 -13.5437 838.773)
; ( -3.34086 -23.3862 4.15149 1114.19)
; ( 0.001829 -0.00128091 -0.0035462 1.0))))
;
;(defvar *j2dlt* (make-array '(3 4) :initial-contents '(
; ( -1.57888 -19.5509 12.7574 1075.15)
; ( 23.0743 0.150066 3.52646 193.678)
; ( 0.000853097 -0.00259802 -0.00302583 1.0))))
;
;(defvar *j3dlt* (make-array '(3 4) :initial-contents '(
; ( -23.9259 -1.6004 -4.50425 930.103)
; ( 1.48448 -24.1476 -2.05204 977.122)
; ( 0.000155764 -0.000260534 -0.00601162 1.0))))
;
;(defvar *j4dlt* (make-array '(3 4) :initial-contents '(
; ( 0.614891 18.9815 -13.7196 188.488)
; ( -23.3171 -0.260518 -1.74393 808.377)
; ( -0.0000450793 -0.00184546 -0.00320554 1.0))))
;
;(defvar *j5dlt* (make-array '(3 4) :initial-contents '(
; ( -0.622512 23.2626 9.83324 314.566)
; ( -25.1722 0.113823 -1.18896 760.496)
; ( -0.000209399 0.00216168 -0.00382999 1.0))))
;
;(defvar *j6dlt* (make-array '(3 4) :initial-contents '(
; ( 2.42274 23.1361 -6.85935 146.434)
; ( -23.2752 0.753822 -6.56249 760.988)
; ( 0.000729179 -0.000783663 -0.00408937 1.0))))
;
;(defvar *j7dlt* (make-array '(3 4) :initial-contents '(
; ( 0.226375 23.4827 -4.05651 197.787)
; ( -23.4512 1.12341 3.73856 831.082)
; ( -0.00103381 -0.000201776 -0.00401774 1.0))))
;
;(defvar *j8dlt* (make-array '(3 4) :initial-contents '(
; ( 0.611087 24.8615 2.77736 179.376)
; ( -17.5396 2.63451 -17.5725 628.414)
; ( 0.00272433 0.000748291 -0.00312889 1.0))))
|#
(defsubst pt-to-nvec (x y)
(la:normalize x y 1.0))
(defsubst nvec-to-pt (nvec)
(list (/ (first nvec) (third nvec)) (/ (second nvec) (third nvec))))
(defsubst nvec-to-pt-values (nvec)
(values (/ (first nvec) (third nvec)) (/ (second nvec) (third nvec))))
(defsubst nvec-to-line (nvec)
(la:normalize2 nvec))
(defsubst endpoints-to-line (pt1 pt2)
(nvec-to-line (la:cross-product (list (car pt1) (cadr pt1) 1.0) (list (car pt2) (cadr pt2) 1.0))))
(defsubst pt-vec-to-line (pt vec)
(nvec-to-line (la:cross-product (list (car pt) (cadr pt) 1.0) vec)))
(defsubst line-intersection-pt (nvec1 nvec2)
(nvec-to-pt (la:cross-product nvec1 nvec2)))
;;;;****************************************************************
;;;; READING INPUT FILES AND INITIALIZATION
(defun read-raw-data-lines
(tksname numrows numcols label &optional (filename "j1-lines-data.dat")
(directory "cicero:/home/vidi/vis_radius_common/MatcherData/"))
"Read a file of data lines in row1 col1 row2 col2 format, and store in x1 y1 x2 y2 format"
(with-open-file (file (format nil "~a~a" directory filename) :direction :input)
(isr2:create tksname
:token-features
'((x1 "" :real)(y1 "" :real)(x2 "" :real)(y2 "" :real))
:frame-features
'((numrows "" :integer) (numcols "" :integer) (label "" :string)))
(setf (isr2:value (isr2:handle (list tksname 'numrows))) (round numrows))
(setf (isr2:value (isr2:handle (list tksname 'numcols))) (round numcols))
(setf (isr2:value (isr2:handle (list tksname 'label))) label)
(do ((row1 (read file nil :eof) (read file nil :eof)))
((eq row1 :eof) tksname)
(let ((col1 (read file))
(row2 (read file))
(col2 (read file)))
(read file) ;;ignore drawproc command
(let ((newtok (isr2:create-new-token tksname)))
(setf (isr2:value (list newtok 'x1)) (coerce col1 'single-float))
(setf (isr2:value (list newtok 'y1)) (coerce (- numrows row1) 'single-float))
(setf (isr2:value (list newtok 'x2)) (coerce col2 'single-float))
(setf (isr2:value (list newtok 'y2)) (coerce (- numrows row2) 'single-float)))))))
(defun convert-raw (&optional (directory "cicero:/home/vidi/vis_radius_common/MatcherData/"))
(read-raw-data-lines 'j1lines 1039 1306 "J1" "j1-lines-data.dat" directory)
(isr2:write-frame 'j1lines "york:rcollins;j1-data-lines.isr2" :all :all)
(read-raw-data-lines 'j2lines 1037 1304 "J2" "j2-lines-data.dat" directory)
(isr2:write-frame 'j2lines "york:rcollins;j2-data-lines.isr2" :all :all)
(read-raw-data-lines 'j3lines 1041 1312 "J3" "j3-lines-data.dat" directory)
(isr2:write-frame 'j3lines "york:rcollins;j3-data-lines.isr2" :all :all)
(read-raw-data-lines 'j4lines 1041 1304 "J4" "j4-lines-data.dat" directory)
(isr2:write-frame 'j4lines "york:rcollins;j4-data-lines.isr2" :all :all)
(read-raw-data-lines 'j5lines 1039 1316 "J5" "j5-lines-data.dat" directory)
(isr2:write-frame 'j5lines "york:rcollins;j5-data-lines.isr2" :all :all)
(read-raw-data-lines 'j6lines 1038 1308 "J6" "j6-lines-data.dat" directory)
(isr2:write-frame 'j6lines "york:rcollins;j6-data-lines.isr2" :all :all)
(read-raw-data-lines 'j7lines 1043 1313 "J7" "j7-lines-data.dat" directory)
(isr2:write-frame 'j7lines "york:rcollins;j7-data-lines.isr2" :all :all)
(read-raw-data-lines 'j8lines 1032 1303 "J8" "j8-lines-data.dat" directory)
(isr2:write-frame 'j8lines "york:rcollins;j8-data-lines.isr2" :all :all)
)
(defun convert-boldt-raw (&optional (directory "cicero:/users1/vis/rcollins/Boldt/"))
(read-raw-data-lines 'j1boldt 1039 1306 "J1b Boldt lines" "j1/j1bboldt.asc" directory)
(isr2:write-frame 'j1boldt "york:rcollins;j1bboldt.isr2" :all :all)
(read-raw-data-lines 'j2boldt 1037 1304 "J2b Boldt lines" "j2/j2bboldt.asc" directory)
(isr2:write-frame 'j2boldt "york:rcollins;j2bboldt.isr2" :all :all)
(read-raw-data-lines 'j3boldt 1041 1312 "J3b Boldt lines" "j3/j3bboldt.asc" directory)
(isr2:write-frame 'j3boldt "york:rcollins;j3bboldt.isr2" :all :all)
(read-raw-data-lines 'j4boldt 1041 1304 "J4b Boldt lines" "j4/j4bboldt.asc" directory)
(isr2:write-frame 'j4boldt "york:rcollins;j4bboldt.isr2" :all :all)
(read-raw-data-lines 'j5boldt 1039 1316 "J5b Boldt lines" "j5/j5bboldt.asc" directory)
(isr2:write-frame 'j5boldt "york:rcollins;j5bboldt.isr2" :all :all)
(read-raw-data-lines 'j6boldt 1038 1308 "J6b Boldt lines" "j6/j6bboldt.asc" directory)
(isr2:write-frame 'j6boldt "york:rcollins;j6bboldt.isr2" :all :all)
(read-raw-data-lines 'j7boldt 1043 1313 "J7b Boldt lines" "j7/j7bboldt.asc" directory)
(isr2:write-frame 'j7boldt "york:rcollins;j7bboldt.isr2" :all :all)
(read-raw-data-lines 'j8boldt 1032 1303 "J8b Boldt lines" "j8/j8bboldt.asc" directory)
(isr2:write-frame 'j8boldt "york:rcollins;j8bboldt.isr2" :all :all)
)
(defun gridify-data-lines (tksname &key (rowsize 32) (colsize 32))
;;note: I'm calling make-grid with cols and rows switched, because later I will
;;access it using (x y) pairs rather than (row col) pairs.
(let* ((grid (isr2:make-grid
tksname
0 (isr2:value (isr2:handle (list tksname 'numcols))) colsize
0 (isr2:value (isr2:handle (list tksname 'numrows))) rowsize))
(gss (isr2:grid-gss grid)))
(isr2:for-every-token (tok tksname (x1 y1 x2 y2))
(isr2:grid-unselect gss)
(isr2:rasterize-line gss (isr2:value x1) (isr2:value y1) (isr2:value x2) (isr2:value y2))
(isr2:grid-store gss (isr2:copy-handle tok)))
grid))
#|
(defun epipolar-initialize ()
(isr2:restore 'j1-data-lines "york:rcollins;j1-data-lines.isr2")
(setf *j1grid* (gridify-data-lines 'j1-data-lines))
(setf *j1numrows* (isr2:value 'j1-data-lines$numrows))
(isr2:restore 'j2-data-lines "york:rcollins;j2-data-lines.isr2")
(setf *j2grid* (gridify-data-lines 'j2-data-lines))
(setf *j2numrows* (isr2:value 'j2-data-lines$numrows))
(isr2:restore 'j3-data-lines "york:rcollins;j3-data-lines.isr2")
(setf *j3grid* (gridify-data-lines 'j3-data-lines))
(setf *j3numrows* (isr2:value 'j3-data-lines$numrows))
(isr2:restore 'j4-data-lines "york:rcollins;j4-data-lines.isr2")
(setf *j4grid* (gridify-data-lines 'j4-data-lines))
(setf *j4numrows* (isr2:value 'j4-data-lines$numrows))
(isr2:restore 'j5-data-lines "york:rcollins;j5-data-lines.isr2")
(setf *j5grid* (gridify-data-lines 'j5-data-lines))
(setf *j5numrows* (isr2:value 'j5-data-lines$numrows))
(isr2:restore 'j6-data-lines "york:rcollins;j6-data-lines.isr2")
(setf *j6grid* (gridify-data-lines 'j6-data-lines))
(setf *j6numrows* (isr2:value 'j6-data-lines$numrows))
(isr2:restore 'j7-data-lines "york:rcollins;j7-data-lines.isr2")
(setf *j7grid* (gridify-data-lines 'j7-data-lines))
(setf *j7numrows* (isr2:value 'j7-data-lines$numrows))
(isr2:restore 'j8-data-lines "york:rcollins;j8-data-lines.isr2")
(setf *j8grid* (gridify-data-lines 'j8-data-lines))
(setf *j8numrows* (isr2:value 'j8-data-lines$numrows))
t)
|#
(defun init-epipolar-data ()
(isr2:restore 'j1-data-lines "york:rcollins;j1bboldt.isr2")
(setf *j1grid* (gridify-data-lines 'j1-data-lines))
(setf *j1numrows* (isr2:value 'j1-data-lines$numrows))
(isr2:restore 'j2-data-lines "york:rcollins;j2bboldt.isr2")
(setf *j2grid* (gridify-data-lines 'j2-data-lines))
(setf *j2numrows* (isr2:value 'j2-data-lines$numrows))
(isr2:restore 'j3-data-lines "york:rcollins;j3bboldt.isr2")
(setf *j3grid* (gridify-data-lines 'j3-data-lines))
(setf *j3numrows* (isr2:value 'j3-data-lines$numrows))
(isr2:restore 'j4-data-lines "york:rcollins;j4bboldt.isr2")
(setf *j4grid* (gridify-data-lines 'j4-data-lines))
(setf *j4numrows* (isr2:value 'j4-data-lines$numrows))
(isr2:restore 'j5-data-lines "york:rcollins;j5bboldt.isr2")
(setf *j5grid* (gridify-data-lines 'j5-data-lines))
(setf *j5numrows* (isr2:value 'j5-data-lines$numrows))
(isr2:restore 'j6-data-lines "york:rcollins;j6bboldt.isr2")
(setf *j6grid* (gridify-data-lines 'j6-data-lines))
(setf *j6numrows* (isr2:value 'j6-data-lines$numrows))
(isr2:restore 'j7-data-lines "york:rcollins;j7bboldt.isr2")
(setf *j7grid* (gridify-data-lines 'j7-data-lines))
(setf *j7numrows* (isr2:value 'j7-data-lines$numrows))
(isr2:restore 'j8-data-lines "york:rcollins;j8bboldt.isr2")
(setf *j8grid* (gridify-data-lines 'j8-data-lines))
(setf *j8numrows* (isr2:value 'j8-data-lines$numrows))
t)
(defun init-epipolar-histograms (&optional (numbuckets *num-z-buckets*) (lowvalue *lowz*) (highvalue *highz*))
(setf *current-histogram* (init-height-histogram numbuckets lowvalue highvalue))
(setf *accum-histogram* (init-height-histogram numbuckets lowvalue highvalue))
(setf *spline-xarr* (make-array numbuckets :element-type 'single-float :initial-element 0.0))
(setf *spline-yarr* (make-array numbuckets :element-type 'single-float :initial-element 0.0))
t)
(defsubst set-label (window string)
(unless *no-labels*
(send window :set-label string)))
(defun init-epipolar-screens ()
(setf *display1* (make-instance 'sg:schema-image-display))
(send *display1* :set-screen-window 0 0 504 395)
(setf *display2* (make-instance 'sg:schema-image-display))
(send *display2* :set-screen-window 512 0 504 395)
(setf *plot1* (make-instance 'sg:schema-image-display))
(send *plot1* :set-screen-window 0 416 504 116)
(set-label *plot1* "accumulated height histogram")
(setf *plot2* (make-instance 'sg:schema-image-display))
(send *plot2* :set-screen-window 512 416 504 116)
(set-label *plot2* "current height histogram")
t)
(defun init-epipolar-display (&key (init-screens t))
(when init-screens
(init-epipolar-screens))
(let ((w *display1*)
(w2 *display2*))
(display-line-tokenset w 'j8-data-lines :init t)
(send w :take-snapshot)
(send w :store-display 'j8lines)
(display-line-tokenset w 'j6-data-lines :init t)
(send w :take-snapshot)
(send w :store-display 'j6lines)
(display-line-tokenset w 'j3-data-lines :init t)
(send w :take-snapshot)
(send w :store-display 'j3lines)
(display-line-tokenset w2 'j8-data-lines :init t)
(send w2 :take-snapshot)
(send w2 :store-display 'j8lines)
(display-line-tokenset w2 'j7-data-lines :init t)
(send w2 :take-snapshot)
(send w2 :store-display 'j7lines)
(display-line-tokenset w2 'j6-data-lines :init t)
(send w2 :take-snapshot)
(send w2 :store-display 'j6lines)
(display-line-tokenset w2 'j5-data-lines :init t)
(send w2 :take-snapshot)
(send w2 :store-display 'j5lines)
(display-line-tokenset w2 'j4-data-lines :init t)
(send w2 :take-snapshot)
(send w2 :store-display 'j4lines)
(display-line-tokenset w2 'j3-data-lines :init t)
(send w2 :take-snapshot)
(send w2 :store-display 'j3lines)
(display-line-tokenset w2 'j2-data-lines :init t)
(send w2 :take-snapshot)
(send w2 :store-display 'j2lines)
(display-line-tokenset w2 'j1-data-lines :init t)
(send w2 :take-snapshot)
(send w2 :store-display 'j1lines)
t))
;;;;****************************************************************
;;;; DLT - DIRECT LINEAR TRANSFORM
(defun dlt-to-amat-bvec (dlt)
(values (make-array '(3 3) :initial-contents
(list (list (aref dlt 0 0) (aref dlt 0 1) (aref dlt 0 2))
(list (aref dlt 1 0) (aref dlt 1 1) (aref dlt 1 2))
(list (aref dlt 2 0) (aref dlt 2 1) (aref dlt 2 2))))
(list (aref dlt 0 3) (aref dlt 1 3) (aref dlt 2 3))))
(defun amat-bvec-to-dlt (amat bvec)
(make-array '(3 4)
:initial-contents
(list (list (aref amat 0 0) (aref amat 0 1) (aref amat 0 2) (elt bvec 0))
(list (aref amat 1 0) (aref amat 1 1) (aref amat 1 2) (elt bvec 1))
(list (aref amat 2 0) (aref amat 2 1) (aref amat 2 2) (elt bvec 2)))))
(defun inverse-dlt (dlt)
(multiple-value-bind (amat bvec) (dlt-to-amat-bvec dlt)
(let ((invamat (solve:inverse amat)))
(amat-bvec-to-dlt invamat (la:vs -1.0 (la:m* invamat bvec))))))
(defvar *j1inverse* (inverse-dlt *j1dlt*))
(defvar *j2inverse* (inverse-dlt *j2dlt*))
(defvar *j3inverse* (inverse-dlt *j3dlt*))
(defvar *j4inverse* (inverse-dlt *j4dlt*))
(defvar *j5inverse* (inverse-dlt *j5dlt*))
(defvar *j6inverse* (inverse-dlt *j6dlt*))
(defvar *j7inverse* (inverse-dlt *j7dlt*))
(defvar *j8inverse* (inverse-dlt *j8dlt*))
(defun dlt-3d-to-2d (dlt x y z)
"Projects 3D world point x y z into image plane using given dlt projective transformation matrix"
(nvec-to-pt (listarray (la:m* dlt (list x y z 1.0)))))
(defun dlt-2d-to-3d (dlt x y &optional (dlt-inv (inverse-dlt dlt)))
"Backprojects 2d image point x y into world ray using inverse of dlt projective transformation
matrix. The resulting world ray is two vectors a and b describing the line k a + b."
(values (listarray (la:m* dlt-inv (list x y 1.0 0.0)))
(listarray (la:m* dlt-inv (list 0.0 0.0 0.0 1.0)))))
;;;;****************************************************************
;;;; COMPUTING THE CROSS RATIO AND SOLVING FOR HEIGHT
;(defun cross-ratio-of-unit-vectors (u1 u2 u3 u4)
; "Returns cross ratio of four coplanar 3d unit vectors."
; (let ((c13 (la:dot-product u1 u3))
; (c14 (la:dot-product u1 u4))
; (c23 (la:dot-product u2 u3))
; (c24 (la:dot-product u2 u4)))
; (print (list c13 c14 c23 c24))
; (let ((s13sq (- 1.0 (* c13 c13)))
; (s14sq (- 1.0 (* c14 c14)))
; (s23sq (- 1.0 (* c23 c23)))
; (s24sq (- 1.0 (* c24 c24))))
; (sqrt (* (/ s13sq s23sq) (/ s24sq s14sq))))))
(defsubst height-cross-ratio (1overcamz lowz highz zval)
(* (/ (- 1.0 (* highz 1overcamz)) (- 1.0 (* zval 1overcamz)))
(/ (- zval lowz) (- highz lowz))))
(defun cr-reference-line (lowline epi2line highline epi1line)
(let* ((midline (la:normalize2 (la:v- lowline highline)))
(epimidline (la:normalize2 (la:v- epi2line epi1line)))
(center (line-intersection-pt midline epimidline)))
(list (- (second midline))
(first midline)
(- (* (second midline) (car center))
(* (first midline) (cadr center))))))
(defun cr-reference-coord (cr-reference-line line)
"returns homogeneous coordinates of intersection point on cross ratio reference line"
(let ((pt (la:unit-normal line cr-reference-line))
(cos (car cr-reference-line))
(sin (cadr cr-reference-line)))
(list (- (* cos (second pt)) (* sin (first pt)))
(third pt))))
(defun cr-reference-value (cr-reference-line line &key (reciprocal? nil))
(multiple-value-bind (x y) (values-list (cr-reference-coord cr-reference-line line))
(if reciprocal?
(/ y x)
(/ x y))))
(defun solve-for-height-given-cr (crvalue 1overcamz lowz highz)
(let ((tmp (* crvalue (- highz lowz))))
(/ (+ tmp lowz (* (- highz) lowz 1overcamz))
(+ 1.0 (* (- tmp highz) 1overcamz)))))
(defun solve-for-cr-given-height (heightvalue 1overcamz lowz highz)
(height-cross-ratio 1overcamz lowz highz heightvalue))
;;;;****************************************************************
;;;; HISTOGRAMMING HEIGHT VALUES
(defun init-height-histogram (&optional (numbuckets *num-z-buckets*) (lowvalue *lowz*) (highvalue *highz*))
(hist:make-histogram :num-buckets numbuckets :min-value lowvalue :max-value highvalue))
(defun clear-height-histogram (&optional (histogram *current-histogram*))
(let ((arr (hist:histogram-array histogram)))
(dotimes (i (length arr) histogram)
(setf (aref arr i) 0.0))))
(defun add-to-height-histogram (hist1 hist2)
"destructively add hist2 into hist1"
(let ((arr1 (hist:histogram-array hist1))
(arr2 (hist:histogram-array hist2)))
(dotimes (i (length arr1) hist2)
(incf (aref arr1 i) (aref arr2 i)))))
(defsubst cross-ratio-CDF (crvalue crmin crmax)
(cond ((<= crvalue crmin) 0.0)
((< crvalue crmax) (/ (- crvalue crmin) (- crmax crmin)))
(t 1.0)))
(defun probability-of-cross-ratio-range (cra crb crmin crmax)
(abs (- (cross-ratio-CDF crb crmin crmax)
(cross-ratio-CDF cra crmin crmax))))
(defun vote-for-height-range (cr1 cr2 1overcamz lowz highz &key (weight 1) (histogram *current-histogram*))
(when (< cr2 cr1) (rotatef cr1 cr2))
(let ((height1 (solve-for-height-given-cr cr1 1overcamz lowz highz))
(height2 (solve-for-height-given-cr cr2 1overcamz lowz highz))
(arr (hist:histogram-array histogram)))
(when (< height2 height1) (rotatef height1 height2))
(let ((lowindex (hist:value-to-index histogram height1))
(highindex (hist:value-to-index histogram height2)))
(do ((index lowindex (+ 1 index)))
((> index highindex) (values height1 height2))
(multiple-value-bind (rangelow rangehigh) (values-list (hist:index-to-range histogram index))
(unless rangelow (setf rangelow lowz))
(unless rangehigh (setf rangehigh highz))
(let ((cra (solve-for-cr-given-height rangelow 1overcamz lowz highz))
(crb (solve-for-cr-given-height rangehigh 1overcamz lowz highz)))
(incf (aref arr index)
(* weight (probability-of-cross-ratio-range cra crb cr1 cr2)))))))))
;;;;****************************************************************
;;;; SEARCHING FOR CANDIDATE EPIPOLAR MATCHES
(defun coarse-get-lines-in-quadrilateral (grid pt1 pt2 pt3 pt4)
(let ((gss (isr2:grid-gss grid)))
(isr2:grid-unselect gss)
(isr2:rasterize-polygon gss (list pt1 pt2 pt3 pt4))
(isr2:grid-retrieve gss)))
(defun backproject-point (dlt-inverse x y zval)
"Backproject image point x y and return the 3d point where the backprojection ray
intersects the plane z = zval."
(multiple-value-bind (a b) (dlt-2d-to-3d nil x y dlt-inverse)
(let ((k (/ (- zval (third b)) (third a))))
(la:v+ b (la:vs k a)))))
(defun epipolar-search-area (dlt1-inverse dlt2 x1 y1 x2 y2 &optional (zlow *lowz*) (zhigh *highz*))
(list (apply #'dlt-3d-to-2d dlt2 (backproject-point dlt1-inverse x1 y1 zlow))
(apply #'dlt-3d-to-2d dlt2 (backproject-point dlt1-inverse x2 y2 zlow))
(apply #'dlt-3d-to-2d dlt2 (backproject-point dlt1-inverse x1 y1 zhigh))
(apply #'dlt-3d-to-2d dlt2 (backproject-point dlt1-inverse x2 y2 zhigh))))
(defun sloppy-points (pt1 pt2 trueline endpoint-slop)
(let ((x1 (car pt1))
(y1 (cadr pt1))
(x2 (car pt2))
(y2 (cadr pt2))
(slopx (* (car trueline) endpoint-slop))
(slopy (* (cadr trueline) endpoint-slop)))
(if (minusp (la:dot-product trueline (pt-to-nvec x1 y1)))
(values
(list (- x1 slopx) (- y1 slopy))
(list (+ x2 slopx) (+ y2 slopy)))
(values
(list (+ x1 slopx) (+ y1 slopy))
(list (- x2 slopx) (- y2 slopy))))))
(defun histogram-epipolar-candidates (w grid low1 low2 high1 high2 numrows
&key (deltatheta .1) (relative-length-thresh .25) (histogram *current-histogram*)
(endpoint-slop 1) (prompt t))
(multiple-value-bind (lowline epi2line highline epi1line)
(clip-to-quad-init low1 low2 high2 high1)
; (setf *lowline* lowline *epi2line* epi2line *highline* highline *epi1line* epi1line)
(let* ((vp (la:unit-normal lowline highline))
(epi (la:unit-normal epi1line epi2line))
(vp-epi-line (la:unit-normal vp epi))
(cr-reference-line (cr-reference-line lowline epi2line highline epi1line))
(1overcamz (cr-reference-value cr-reference-line vp-epi-line :reciprocal? t))
(lowz (cr-reference-value cr-reference-line lowline))
(highz (cr-reference-value cr-reference-line highline))
(cos-theta-thresh (cos (abs deltatheta)))
(candidates nil) (line nil) (trueline nil) (relative-len nil))
(setf candidates (coarse-get-lines-in-quadrilateral grid low1 low2 high2 high1))
(when *demo-mode*
(epipolar-display w low1 low2 high1 high2 numrows :zoom t)
; (let ((pt1 (line-intersection-pt cr-reference-line lowline))
; (pt2 (line-intersection-pt cr-reference-line highline)))
; (display-line w (car pt1) (cadr pt1) (car pt2) (cadr pt2) numrows :color w:green)
; (send w :display-point (car pt2) (- numrows (cadr pt2)) :radius 2 :color w:green))
(display-line-tokenset w candidates :color *line-color* :thickness *line-thickness*)
(epipolar-display w low1 low2 high1 high2 numrows :zoom nil))
(isr2:for-every-token (tok candidates (x1 y1 x2 y2))
(multiple-value-bind (x1 y1 x2 y2 ok?)
(clip-to-quad (isr2:value x1) (isr2:value y1) (isr2:value x2) (isr2:value y2)
lowline epi2line highline epi1line)
(when ok?
(let ((pt1 (list x1 y1))
(pt2 (list x2 y2))
(midpt (list (/ (+ x1 x2) 2.0) (/ (+ y1 y2) 2.0))))
(setf line (endpoints-to-line pt1 pt2))
(setf trueline (pt-vec-to-line midpt vp))
(when (< cos-theta-thresh (abs (la:dot-product (firstn 2 line) (firstn 2 trueline))))
(setf relative-len
(/ (vp:line-length x1 y1 x2 y2)
(apply #'(lambda (x1 y1 x2 y2) (vp:line-length x1 y1 x2 y2))
(append (line-intersection-pt trueline epi1line)
(line-intersection-pt trueline epi2line)))))
(when (>= relative-len relative-length-thresh)
(multiple-value-setq (pt1 pt2) (sloppy-points pt1 pt2 trueline endpoint-slop))
(let ((zvalue1 (cr-reference-value cr-reference-line (pt-vec-to-line pt1 vp)))
(zvalue2 (cr-reference-value cr-reference-line (pt-vec-to-line pt2 vp))))
(multiple-value-bind (lowheight highheight)
(vote-for-height-range
(height-cross-ratio 1overcamz lowz highz zvalue1)
(height-cross-ratio 1overcamz lowz highz zvalue2)
*1overcamz* *lowz* *highz*
:weight relative-len
:histogram histogram)
(when prompt
(format t "Token ~s votes ~d for height range (~d,~d)~%"
tok relative-len lowheight highheight)))
(when *demo-mode*
; (display-line w (car pt1) (cadr pt1) (car pt2) (cadr pt2) numrows
; :color w:red :thickness 3)
(display-line w x1 y1 x2 y2 numrows :color *search-color* :thickness *search-thickness*))
()))))))))))
(defun test-single-epipolar-candidate (w grid low1 low2 high1 high2 numrows testx1 testy1 testx2 testy2
&key (deltatheta .1) (relative-length-thresh .25) (histogram *current-histogram*)
(endpoint-slop 1))
(multiple-value-bind (lowline epi2line highline epi1line)
(clip-to-quad-init low1 low2 high2 high1)
; (setf *lowline* lowline *epi2line* epi2line *highline* highline *epi1line* epi1line)
(let* ((vp (la:unit-normal lowline highline))
(epi (la:unit-normal epi1line epi2line))
(vp-epi-line (la:unit-normal vp epi))
(cr-reference-line (cr-reference-line lowline epi2line highline epi1line))
(1overcamz (cr-reference-value cr-reference-line vp-epi-line :reciprocal? t))
(lowz (cr-reference-value cr-reference-line lowline))
(highz (cr-reference-value cr-reference-line highline))
(cos-theta-thresh (cos (abs deltatheta)))
(candidates nil) (line nil) (trueline nil) (relative-len nil))
(setf candidates (coarse-get-lines-in-quadrilateral grid low1 low2 high2 high1))
(epipolar-display w low1 low2 high1 high2 numrows :zoom t)
(let ((pt1 (line-intersection-pt cr-reference-line lowline))
(pt2 (line-intersection-pt cr-reference-line highline)))
(display-line w (car pt1) (cadr pt1) (car pt2) (cadr pt2) numrows :color w:green :thickness 3)
(send w :display-point (car pt2) (- numrows (cadr pt2)) :radius 2 :color w:green))
(display-line-tokenset w candidates)
(multiple-value-bind (x1 y1 x2 y2 ok?)
(clip-to-quad testx1 testy1 testx2 testy2 lowline epi2line highline epi1line)
(when ok?
(let ((pt1 (list x1 y1))
(pt2 (list x2 y2))
(midpt (list (/ (+ x1 x2) 2.0) (/ (+ y1 y2) 2.0))))
(setf line (endpoints-to-line pt1 pt2))
(setf trueline (pt-vec-to-line midpt vp))
(when (< cos-theta-thresh (abs (la:dot-product (firstn 2 line) (firstn 2 trueline))))
(setf relative-len
(/ (vp:line-length x1 y1 x2 y2)
(apply #'(lambda (x1 y1 x2 y2) (vp:line-length x1 y1 x2 y2))
(append (line-intersection-pt trueline epi1line)
(line-intersection-pt trueline epi2line)))))
(when (>= relative-len relative-length-thresh)
(multiple-value-setq (pt1 pt2) (sloppy-points pt1 pt2 trueline endpoint-slop))
(let ((zvalue1 (cr-reference-value cr-reference-line (pt-vec-to-line pt1 vp)))
(zvalue2 (cr-reference-value cr-reference-line (pt-vec-to-line pt2 vp))))
(print (list 1overcamz lowz highz zvalue1))
(print (list 1overcamz lowz highz zvalue2))
(multiple-value-bind (lowheight highheight)
(vote-for-height-range
(height-cross-ratio 1overcamz lowz highz zvalue1)
(height-cross-ratio 1overcamz lowz highz zvalue2)
*1overcamz* *lowz* *highz*
:weight relative-len
:histogram histogram)
(format t "Token ~s votes ~d for height range (~d,~d)~%"
(list testx1 testy1 testx2 testy2) relative-len lowheight highheight)
(format t "cross-ratios ~d ~d~%"
(height-cross-ratio 1overcamz lowz highz zvalue1)
(height-cross-ratio 1overcamz lowz highz zvalue2))
)
(display-line w x1 y1 x2 y2 numrows :color w:blue)
(display-line w (car pt1) (cadr pt1) (car pt2) (cadr pt2) numrows :color w:red)
())))))))))
(defun test-epipolar-search-area (w dlt1-inverse dlt2 x1 y1 x2 y2 grid numrows &key
(deltatheta .1) (relative-length-thresh .25)
(endpoint-slop 1) (testline nil) (prompt t))
(multiple-value-bind (low1 low2 high1 high2)
(values-list (epipolar-search-area dlt1-inverse dlt2 x1 y1 x2 y2))
(when prompt
(epipolar-display w low1 low2 high1 high2 numrows)
(yes-or-no-p "continue"))
(if testline
(test-single-epipolar-candidate
w grid low1 low2 high1 high2 numrows
(car testline) (cadr testline) (caddr testline) (fourth testline)
:deltatheta deltatheta
:relative-length-thresh relative-length-thresh
:endpoint-slop endpoint-slop)
(histogram-epipolar-candidates
w grid low1 low2 high1 high2 numrows
:deltatheta deltatheta
:relative-length-thresh relative-length-thresh
:endpoint-slop endpoint-slop :prompt prompt))))
(defun test-all-epipolar-search-areas (linelist &key (endpoint-slop 1.0)(delta-theta .1)
(prompt t) (prompt-first nil))
(let ((w *display1*)
(w2 *display2*)
(first t))
(clear-height-histogram *accum-histogram*)
(when *demo-mode*
(send *plot1* :clear)
(send *plot2* :clear))
(dolist (line linelist)
(multiple-value-bind (x1 y1 x2 y2) (values-list line)
(when *demo-mode*
(send w :restore-display 'j3lines)
(display-line w x1 y1 x2 y2 *j3numrows* :color *search-color* :thickness (1+ *search-thickness*)))
(mapcar
#'(lambda (tks displayname dlt grid numrows)
(when *demo-mode*
(let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
(numcols (isr2:value (isr2:handle (list tks 'numcols))))
(label (isr2:value (isr2:handle (list tks 'label)))))
(set-label w2 label)
(send w2 :set-image-window 0 0 numcols numrows)))
(when (and prompt *demo-mode*)
(send w2 :restore-display displayname))
(clear-height-histogram *current-histogram*)
(test-epipolar-search-area w2 *j3inverse* dlt x1 y1 x2 y2 grid numrows
:deltatheta delta-theta :relative-length-thresh .1
:endpoint-slop endpoint-slop :prompt prompt)
(when *demo-mode*
(plot-histogram *plot2* *current-histogram* :max 1))
(add-to-height-histogram *accum-histogram* *current-histogram*)
(when *demo-mode*
(plot-histogram *plot1* *accum-histogram* :max 3))
(when (and first prompt-first)
(yes-or-no-p "continue"))
(setf first nil)
(when prompt
(do ((done (yes-or-no-p "next image") (yes-or-no-p "next image")))
(done t)
(print "enter line coordinate list (x1 y1 x2 y2)")
(let ((testline (eval (read))))
(test-epipolar-search-area w2 *j3inverse* dlt x1 y1 x2 y2 grid numrows
:deltatheta delta-theta :relative-length-thresh .1
:testline testline
:endpoint-slop endpoint-slop)))))
(list 'j1-data-lines 'j2-data-lines 'j4-data-lines
'j5-data-lines 'j6-data-lines 'j7-data-lines 'j8-data-lines)
(list 'j1lines 'j2lines 'j4lines 'j5lines 'j6lines 'j7lines 'j8lines)
(list *j1dlt* *j2dlt* *j4dlt* *j5dlt* *j6dlt* *j7dlt* *j8dlt*)
(list *j1grid* *j2grid* *j4grid* *j5grid* *j6grid* *j7grid* *j8grid*)
(list *j1numrows* *j2numrows* *j4numrows* *j5numrows* *j6numrows* *j7numrows* *j8numrows*))))
*accum-histogram*))
;;;;****************************************************************
;;;; CLIPPING TO A QUADRILATERAL
(defsubst ccw-p (x1 y1 x2 y2 x3 y3)
" Returns T iff (x1,y1)-(x2,y2)-(x3,y3)-(x1,y1) is a counterclockwise walk, NIL otherwise."
;;;;Note: computed by taking the determinant of the matrix
;;;; x1 y1 1
;;;; x2 y2 1
;;;; x3 y3 1
;;;;the determinant is positive iff p1-p2-p3 form a CCW cycle
(plusp (+ (* x1 (- y2 y3))
(* y1 (- x3 x2))
(- (* x2 y3) (* y2 x3)))))
(defun clip-to-quad-init (pt1 pt2 pt3 pt4)
" Initialize the four homogeneous coordinate line vectors in the form needed
by clip-to-quad-aux. The given points must represent a walk around the quadrilateral."
(unless (apply #'ccw-p (append pt1 pt2 pt3))
(rotatef pt1 pt2)
(rotatef pt3 pt4))
(values (endpoints-to-line pt1 pt2) ;\
(endpoints-to-line pt2 pt3) ; \ a CCW walk around the quadrilateral
(endpoints-to-line pt3 pt4) ; /
(endpoints-to-line pt4 pt1) ;/
))
(defconstant topclipcode 8)
(defconstant botclipcode 4)
(defconstant rightclipcode 2)
(defconstant leftclipcode 1)
;(defparameter *small-negative-distance* -1e-6)
;
;(defsubst clip-to-quad-outcodes (x y top left bot right)
; (let ((vec (list x y 1.0)))
; (print (list x y (la:dot-product top vec) (la:dot-product bot vec)
; (la:dot-product right vec) (la:dot-product left vec)))
; (+
; (if (< (la:dot-product top vec) *small-negative-distance*) topclipcode 0)
; (if (< (la:dot-product bot vec) *small-negative-distance*) botclipcode 0)
; (if (< (la:dot-product right vec) *small-negative-distance*) rightclipcode 0)
; (if (< (la:dot-product left vec) *small-negative-distance*) leftclipcode 0))))
(defsubst clip-to-quad-outcodes (x y top left bot right)
(let ((vec (list x y 1.0)))
; (print (list x y (la:dot-product top vec) (la:dot-product bot vec)
; (la:dot-product right vec) (la:dot-product left vec)))
(+
(if (minusp (la:dot-product top vec)) topclipcode 0)
(if (minusp (la:dot-product bot vec)) botclipcode 0)
(if (minusp (la:dot-product right vec)) rightclipcode 0)
(if (minusp (la:dot-product left vec)) leftclipcode 0))))
(defsubst clip-to-quad-line-intersect (line clipline)
(nvec-to-pt-values (la:cross-product line clipline)))
;(defsubst clip-to-quad-line-intersect (line clipline)
; (values-list (listarray
; (solve:linear-system
; (make-array '(2 2) :initial-contents (list (firstn 2 line) (firstn 2 clipline)))
; (vector (- (third line)) (- (third clipline)))))))
(defsubst clip-to-quad-reject-check (outcode1 outcode2)
(not (zerop (logand outcode1 outcode2))))
(defsubst clip-to-quad-accept-check (outcode1 outcode2)
(and (zerop outcode1) (zerop outcode2)))
(defun clip-to-quad (x1 y1 x2 y2 top left bot right)
"Clip line x1 y1 x2 y2 to a quadrilateral defined by homogeneous coordinate
lines top,left,bot,right which are calculated by clip-to-quad-init."
(let ((accept nil) (reject nil) (ignorecode1 0) (ignorecode2 0)
(line (endpoints-to-line (list x1 y1) (list x2 y2))))
; (print (list x1 y1 x2 y2 top left bot right))
; (multiple-value-setq (pt1 pt2) (values (list x1 y1) (list x2 y2)))
; (multiple-value-setq (l1 l2 l3 l4) (values top bot right left))
(do ((done nil))
(done t) ;;loop until done
(let ((outcode1 (logandc1 ignorecode1 (clip-to-quad-outcodes x1 y1 top left bot right)))
(outcode2 (logandc1 ignorecode2 (clip-to-quad-outcodes x2 y2 top left bot right))))
; (print (list outcode1 outcode2))
(setf reject (clip-to-quad-reject-check outcode1 outcode2))
(if reject
(setf done t)
(progn
(setf accept (clip-to-quad-accept-check outcode1 outcode2))
(if accept
(setf done t)
(progn
(when (= outcode1 0)
(rotatef x1 x2)
(rotatef y1 y2)
(rotatef outcode1 outcode2)
(rotatef ignorecode1 ignorecode2))
(cond
((logtest topclipcode outcode1)
(multiple-value-setq (x1 y1) (clip-to-quad-line-intersect line top))
(incf ignorecode1 topclipcode))
((logtest botclipcode outcode1)
(multiple-value-setq (x1 y1) (clip-to-quad-line-intersect line bot))
(incf ignorecode1 botclipcode))
((logtest rightclipcode outcode1)
(multiple-value-setq (x1 y1) (clip-to-quad-line-intersect line right))
(incf ignorecode1 rightclipcode))
((logtest leftclipcode outcode1)
(multiple-value-setq (x1 y1) (clip-to-quad-line-intersect line left))
(incf ignorecode1 leftclipcode)))))))))
(if accept
(values x1 y1 x2 y2 t)
(values 0 0 0 0 nil))))
(defun test-clip-to-quad (w pt1 pt2 pt3 pt4)
(declare (special l1 l2 l3 l4))
(send w :clear)
(send w :display-line (car pt1) (cadr pt1) (car pt2) (cadr pt2) :color w:black)
(send w :display-line (car pt2) (cadr pt2) (car pt3) (cadr pt3) :color w:black)
(send w :display-line (car pt3) (cadr pt3) (car pt4) (cadr pt4) :color w:black)
(send w :display-line (car pt4) (cadr pt4) (car pt1) (cadr pt1) :color w:black)
(multiple-value-setq (l1 l2 l3 l4)
(clip-to-quad-init pt1 pt2 pt3 pt4))
(loop
(let ((input (list (random 10.0) (random 10.0) (random 10.0) (random 10.0))))
(multiple-value-bind (x1 y1 x2 y2 ok)
(clip-to-quad (car input) (cadr input) (third input) (fourth input) l1 l2 l3 l4)
(when ok
(send w :display-line x1 y1 x2 y2 :color w:green))))))
;;;;****************************************************************
;;;; DISPLAY ROUTINES
(defun display-line-tokenset (w tks &key (init nil) (color w:black) (thickness 1))
(setf tks (isr2:handle tks))
(let ((numrows (isr2:value (isr2:handle (list (isr2:frame tks) 'numrows)))))
(when init
(let ((numcols (isr2:value (isr2:handle (list (isr2:frame tks) 'numcols))))
(label (isr2:value (isr2:handle (list (isr2:frame tks) 'label)))))
(set-label w label)
(send w :clear)
(send w :set-image-window 0 0 numcols numrows)))
(isr2:for-every-token (tok tks (x1 y1 x2 y2))
(send w :display-line (isr2:value x1) (- numrows (isr2:value y1))
(isr2:value x2) (- numrows (isr2:value y2)) :color color :thickness thickness))))
(defun display-line-token (w token numrows &key (color w:black) (thickness 1))
(send w :display-line
(isr2:value (list token 'x1))
(- numrows (isr2:value (list token 'y1)))
(isr2:value (list token 'x2))
(- numrows (isr2:value (list token 'y2)))
:color color :thickness thickness))
(defun display-line (w x1 y1 x2 y2 numrows &key (color w:black) (thickness 1))
(send w :display-line x1 (- numrows y1) x2 (- numrows y2) :color color :thickness thickness))
(defun epipolar-display (w low1 low2 high1 high2 numrows &key (zoom nil) (boundary-pixels 5)
(color *epipolar-color*) (thickness *epipolar-thickness*))
(multiple-value-bind (x1 y1) (values-list low1)
(multiple-value-bind (x2 y2) (values-list low2)
(multiple-value-bind (x3 y3) (values-list high1)
(multiple-value-bind (x4 y4) (values-list high2)
(setf y1 (- numrows y1))
(setf y2 (- numrows y2))
(setf y3 (- numrows y3))
(setf y4 (- numrows y4))
(when zoom
(send w :clear)
(let ((minx (min x1 x2 x3 x4))
(maxx (max x1 x2 x3 x4))
(miny (min y1 y2 y3 y4))
(maxy (max y1 y2 y3 y4)))
(let* ((dx (- maxx minx))
(dy (- maxy miny))
(maxd (max dx dy)))
(send w :set-image-window
(floor (- (/ (+ minx maxx) 2) (/ maxd 2) boundary-pixels))
(floor (- (/ (+ miny maxy) 2) (/ maxd 2) boundary-pixels))
(ceiling (+ maxd (* 2 boundary-pixels)))))))
(send w :display-line x1 y1 x2 y2 :color color :thickness thickness)
(send w :display-line x2 y2 x4 y4 :color color :thickness thickness)
(send w :display-line x4 y4 x3 y3 :color color :thickness thickness)
(send w :display-line x3 y3 x1 y1 :color color :thickness thickness))))))
(defun plot-histogram (window histogram &key min max (numknots 3) (clear t)
(thickness *histogram-thickness*) (color *histogram-color*))
" Plot histogram as a spline on the given window. Returns as multiple values the
histogram array, its minimum, and its maximum element values."
(let ((sequence (hist:histogram-array histogram)))
(let ((max (max (if max max 0) (reduce #'max sequence)))
(min (min (if min min 0) (reduce #'min sequence)))
(size (length sequence)))
(when clear (send window :clear))
(send window :set-image-window 0 (min 0 min) size (- max min))
(dotimes (i size)
(setf (aref *spline-xarr* i) (+ 0.5 i))
(setf (aref *spline-yarr* i) (- max (aref sequence i))))
(send window :display-spline *spline-xarr* *spline-yarr* numknots :color color :thickness thickness)
(values sequence min max))))
;;;;****************************************************************
;;;; MISCELLANEOUS TESTING ROUTINES
#|
(defun special-test-histogram-epipolar-candidates (w grid low1 low2 high1 high2 numrows x1 y1 x2 y2
&optional (deltatheta .1) (relative-length-thresh .25))
(multiple-value-bind (lowline epi2line highline epi1line)
(clip-to-quad-init low1 low2 high2 high1)
; (setf *lowline* lowline *epi2line* epi2line *highline* highline *epi1line* epi1line)
(let* ((vp (la:unit-normal lowline highline))
(epi (la:unit-normal epi1line epi2line))
(vp-epi-line (la:unit-normal vp epi))
(cr-reference-line (cr-reference-line lowline epi2line highline epi1line))
(1overcamz (cr-reference-value cr-reference-line vp-epi-line :reciprocal? t))
(lowz (cr-reference-value cr-reference-line lowline))
(highz (cr-reference-value cr-reference-line highline))
(cos-theta-thresh (cos (abs deltatheta)))
(candidates nil) (line nil) (trueline nil) (relative-len nil))
(setf candidates (coarse-get-lines-in-quadrilateral grid low1 low2 high2 high1))
(epipolar-display w low1 low2 high1 high2 numrows :zoom t)
(let ((pt1 (line-intersection-pt cr-reference-line lowline))
(pt2 (line-intersection-pt cr-reference-line highline)))
(display-line w (car pt1) (cadr pt1) (car pt2) (cadr pt2) numrows :color w:green)
(send w :display-point (car pt2) (- numrows (cadr pt2)) :radius 2 :color w:green))
(display-line-tokenset w candidates)
(multiple-value-bind (x1 y1 x2 y2 ok?)
(clip-to-quad x1 y1 x2 y2 lowline epi2line highline epi1line)
(when ok?
(let ((pt1 (list x1 y1))
(pt2 (list x2 y2))
(midpt (list (/ (+ x1 x2) 2.0) (/ (+ y1 y2) 2.0))))
(setf line (endpoints-to-line pt1 pt2))
(setf trueline (pt-vec-to-line midpt vp))
(when (< cos-theta-thresh (abs (la:dot-product (firstn 2 line) (firstn 2 trueline))))
(setf relative-len
(/ (vp:line-length x1 y1 x2 y2)
(apply #'(lambda (x1 y1 x2 y2) (vp:line-length x1 y1 x2 y2))
(append (line-intersection-pt trueline epi1line)
(line-intersection-pt trueline epi2line)))))
(when (>= relative-len relative-length-thresh)
(format t "Token ~s relative-len ~d~%" (list x1 y1 x2 y2) relative-len)
(format t "cross-ratios ~d ~d~%"
(height-cross-ratio
1overcamz lowz highz
(cr-reference-value cr-reference-line (pt-vec-to-line pt1 vp)))
(height-cross-ratio
1overcamz lowz highz
(cr-reference-value cr-reference-line (pt-vec-to-line pt2 vp))))
(display-line w x1 y1 x2 y2 numrows :color w:blue)
()))))))))
(defun special-test-epipolar-search-area (w dlt1-inverse dlt2 x1 y1 x2 y2 testx1 testy1 testx2 testy2
grid numrows &optional
(deltatheta .1) (relative-length-thresh .25))
(multiple-value-bind (low1 low2 high1 high2)
(values-list (epipolar-search-area dlt1-inverse dlt2 x1 y1 x2 y2))
(epipolar-display w low1 low2 high1 high2 numrows)
(yes-or-no-p "continue")
(special-test-histogram-epipolar-candidates
w grid low1 low2 high1 high2 numrows
testx1 testy1 testx2 testy2
deltatheta relative-length-thresh)))
(defun special-test-all-epipolar-search-areas (tx1 ty1 tz1 tx2 ty2 tz2)
(send w :restore-display 'j3lines)
(multiple-value-bind (x1 y1) (values-list (dlt-3d-to-2d *j3dlt* tx1 ty1 tz1))
(multiple-value-bind (x2 y2) (values-list (dlt-3d-to-2d *j3dlt* tx2 ty2 tz2))
(display-line w x1 y1 x2 y2 *j3numrows* :color w:red)
(mapcar
#'(lambda (tks displayname dlt grid numrows)
(let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
(numcols (isr2:value (isr2:handle (list tks 'numcols))))
(label (isr2:value (isr2:handle (list tks 'label)))))
(set-label w2 label)
(send w2 :set-image-window 0 0 numcols numrows))
(send w2 :restore-display displayname)
(let ((pt1 (dlt-3d-to-2d dlt tx1 ty1 tz1))
(pt2 (dlt-3d-to-2d dlt tx2 ty2 tz2)))
(special-test-epipolar-search-area
w2 *j3inverse* dlt x1 y1 x2 y2
(car pt1) (cadr pt1) (car pt2) (cadr pt2)
grid numrows .1 .1))
(yes-or-no-p "next image"))
(list 'j1-data-lines 'j2-data-lines 'j4-data-lines
'j5-data-lines 'j6-data-lines 'j7-data-lines 'j8-data-lines)
(list 'j1lines 'j2lines 'j4lines 'j5lines 'j6lines 'j7lines 'j8lines)
(list *j1dlt* *j2dlt* *j4dlt* *j5dlt* *j6dlt* *j7dlt* *j8dlt*)
(list *j1grid* *j2grid* *j4grid* *j5grid* *j6grid* *j7grid* *j8grid*)
(list *j1numrows* *j2numrows* *j4numrows* *j5numrows* *j6numrows* *j7numrows* *j8numrows*)))))
|#
;;;================================================================================================
#|
(defun project-polygon (w dlt numrows color p1 p2 p3 p4 )
(let ((a1 (dlt-3d-to-2d dlt (car p1) (cadr p1) (caddr p1)))
(a2 (dlt-3d-to-2d dlt (car p2) (cadr p2) (caddr p2)))
(a3 (dlt-3d-to-2d dlt (car p3) (cadr p3) (caddr p3)))
(a4 (dlt-3d-to-2d dlt (car p4) (cadr p4) (caddr p4))))
(display-line w (car a1) (cadr a1) (car a2) (cadr a2) numrows :color color )
(display-line w (car a2) (cadr a2) (car a3) (cadr a3) numrows :color color )
(display-line w (car a3) (cadr a3) (car a4) (cadr a4) numrows :color color )
(display-line w (car a4) (cadr a4) (car a1) (cadr a1) numrows :color color )))
(setf p1
'((14.460031 26.344957 -0.75287825)
(14.324111 16.400723 -0.75287825)
(4.433068 16.535915 -0.75287825)
(4.568989 26.48015 -0.75287825)))
(setf p2
'((17.193123 13.460842 -0.079968385)
(17.193123 0.012386719 -0.079968385)
(-0.071631595 0.012386719 -0.079968385)
(-0.071631595 13.460842 -0.079968385)))
(defvar *m1* (make-array '(3 4) :initial-contents
'((5.701046 23.519785 -1.347012 38.638567)
(-3.078465 2.351684 23.871563 529.155831)
(-0.003835 0.001376 -0.000270 1.0))))
(defvar *m2* (make-array '(3 4) :initial-contents
'((-1.518658 23.442719 -0.512469 58.146075)
(-2.813365 0.551124 23.268698 555.899317)
(-0.003952 0.000177 -0.000135 1.0))))
(defvar *m3* (make-array '(3 4) :initial-contents
'((-14.561851 16.780197 -0.572034 256.198825)
(-3.085134 -1.649948 21.897297 582.343148)
(-0.003069 -0.002128 -0.000262 1.0))))
(defvar *m4* (make-array '(3 4) :initial-contents
'((-21.566046 -2.063989 -0.517090 750.542381)
(-0.432663 -3.013118 21.406709 590.406298)
(-0.000049 -0.003645 -0.000193 1.0))))
(defvar *m5* (make-array '(3 4) :initial-contents
'((-11.110956 -20.023827 -0.556989 1184.024421)
(2.443573 -2.234966 22.614049 585.633526)
(0.003146 -0.002224 -0.000220 1.0))))
(defvar *m6* (make-array '(3 4) :initial-contents
'((6.500043 -23.873375 -0.610927 1243.099897)
(3.658103 0.126230 24.421587 563.681390)
(0.004111 0.000651 -0.000252 1.0))))
(defvar *m7* (make-array '(3 4) :initial-contents
'((26.021084 4.941464 -0.575666 467.823417)
(0.229142 3.205019 26.236815 544.716835)
(-0.000349 0.004447 -0.000148 1.0))))
(defvar *m8* (make-array '(3 4) :initial-contents
'((18.152642 17.996889 -0.610551 153.248636)
(-1.858501 3.078040 25.256988 536.382476)
(-0.002679 0.003366 -0.000228 1.0))))
(defun project-polygon-vertices (dlt point-list)
(mapcar #'(lambda (p) (dlt-3d-to-2d dlt (car p) (cadr p) (caddr p)))
point-list))
(defun project-polygons-postscript-output (stream dlt polygon-list)
(terpri stream)
(dolist (poly polygon-list)
(format stream "%%polygon index: ~d~%newpath~%" (car poly))
(let ((points (project-polygon-vertices dlt (cadr poly))))
(format stream "~d ~d moveto~%" (car (car points)) (cadr (car points)))
(dolist (pt (cdr points))
(format stream "~d ~d lineto~%" (car pt) (cadr pt)))
(format stream "closepath stroke~%"))))
(defun project-2d-polygons-postscript-output (stream polygon-list &aux (index 0))
(terpri stream)
(dolist (poly polygon-list)
(format stream "%%polygon index: ~d~%newpath~%" (incf index))
(let ((points poly))
(format stream "~d ~d moveto~%" (car (car points)) (cadr (car points)))
(dolist (pt (cdr points))
(format stream "~d ~d lineto~%" (car pt) (cadr pt)))
(format stream "closepath stroke~%"))))
(defun read-poly (stream)
(let ((result nil))
(dotimes (i (read stream) (reverse result))
(push (list (read stream) (read stream) (read stream)) result))))
(defun read-xg-format-polygons (filename)
(let ((result nil))
(with-open-file (file filename :direction :input)
(let ((numpolygons (read file)))
(dotimes (i numpolygons (cdr (reverse result)))
(push (list (read file) (read-poly file)) result))))))
|#
#|
(defvar *line-matches* nil)
(defun j1-test-all-epipolar-search-areas (linelist &key (endpoint-slop 1.0) (delta-theta .1)
(prompt t) (prompt-first nil))
(let ((w *display1*)
(w2 *display2*)
(first t))
(setf *line-matches* nil)
(clear-height-histogram *accum-histogram*)
(send *plot1* :clear)
(send *plot2* :clear)
(dolist (line linelist)
(push line *line-matches*)
(multiple-value-bind (x1 y1 x2 y2) (values-list line)
(send w :restore-display 'j1lines)
(display-line w x1 y1 x2 y2 *j1numrows* :color *search-color* :thickness (1+ *search-thickness*))
(mapcar
#'(lambda (tks displayname dlt grid numrows)
(let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
(numcols (isr2:value (isr2:handle (list tks 'numcols))))
(label (isr2:value (isr2:handle (list tks 'label)))))
(set-label w2 label)
(send w2 :set-image-window 0 0 numcols numrows))
(when prompt
(send w2 :restore-display displayname))
(clear-height-histogram *current-histogram*)
(test-epipolar-search-area w2 *j1inverse* dlt x1 y1 x2 y2 grid numrows
:deltatheta delta-theta :relative-length-thresh .1
:endpoint-slop endpoint-slop :prompt prompt)
(plot-histogram *plot2* *current-histogram* :max 1)
(add-to-height-histogram *accum-histogram* *current-histogram*)
(plot-histogram *plot1* *accum-histogram* :max 3)
(when (and first prompt-first)
(yes-or-no-p "continue"))
(setf first nil)
(when prompt
(do ((done (yes-or-no-p "next image") (yes-or-no-p "next image")))
(done t)
(print "enter line coordinate list (x1 y1 x2 y2)")
(let ((testline (eval (read))))
(test-epipolar-search-area w2 *j1inverse* dlt x1 y1 x2 y2 grid numrows
:deltatheta delta-theta
:relative-length-thresh .1 :testline testline
:endpoint-slop endpoint-slop)))))
(list 'j3-data-lines 'j2-data-lines 'j4-data-lines
'j5-data-lines 'j6-data-lines 'j7-data-lines 'j8-data-lines)
(list 'j3lines 'j2lines 'j4lines 'j5lines 'j6lines 'j7lines 'j8lines)
(list *j3dlt* *j2dlt* *j4dlt* *j5dlt* *j6dlt* *j7dlt* *j8dlt*)
(list *j3grid* *j2grid* *j4grid* *j5grid* *j6grid* *j7grid* *j8grid*)
(list *j3numrows* *j2numrows* *j4numrows* *j5numrows* *j6numrows* *j7numrows* *j8numrows*))))
*accum-histogram*))
(defun histogram-epipolar-candidates (w grid low1 low2 high1 high2 numrows
&key (deltatheta .1) (relative-length-thresh .25) (histogram *current-histogram*)
(endpoint-slop 1) (prompt t))
(multiple-value-bind (lowline epi2line highline epi1line)
(clip-to-quad-init low1 low2 high2 high1)
; (setf *lowline* lowline *epi2line* epi2line *highline* highline *epi1line* epi1line)
(let* ((vp (la:unit-normal lowline highline))
(epi (la:unit-normal epi1line epi2line))
(vp-epi-line (la:unit-normal vp epi))
(cr-reference-line (cr-reference-line lowline epi2line highline epi1line))
(1overcamz (cr-reference-value cr-reference-line vp-epi-line :reciprocal? t))
(lowz (cr-reference-value cr-reference-line lowline))
(highz (cr-reference-value cr-reference-line highline))
(cos-theta-thresh (cos (abs deltatheta)))
(candidates nil) (line nil) (trueline nil) (relative-len nil))
(setf candidates (coarse-get-lines-in-quadrilateral grid low1 low2 high2 high1))
(epipolar-display w low1 low2 high1 high2 numrows :zoom t)
; (let ((pt1 (line-intersection-pt cr-reference-line lowline))
; (pt2 (line-intersection-pt cr-reference-line highline)))
; (display-line w (car pt1) (cadr pt1) (car pt2) (cadr pt2) numrows :color w:green)
; (send w :display-point (car pt2) (- numrows (cadr pt2)) :radius 2 :color w:green))
(display-line-tokenset w candidates :color *line-color* :thickness *line-thickness*)
(epipolar-display w low1 low2 high1 high2 numrows :zoom nil)
(isr2:for-every-token (tok candidates (x1 y1 x2 y2))
(multiple-value-bind (x1 y1 x2 y2 ok?)
(clip-to-quad (isr2:value x1) (isr2:value y1) (isr2:value x2) (isr2:value y2)
lowline epi2line highline epi1line)
(when ok?
(let ((pt1 (list x1 y1))
(pt2 (list x2 y2))
(midpt (list (/ (+ x1 x2) 2.0) (/ (+ y1 y2) 2.0))))
(setf line (endpoints-to-line pt1 pt2))
(setf trueline (pt-vec-to-line midpt vp))
(when (< cos-theta-thresh (abs (la:dot-product (firstn 2 line) (firstn 2 trueline))))
(setf relative-len
(/ (vp:line-length x1 y1 x2 y2)
(apply #'(lambda (x1 y1 x2 y2) (vp:line-length x1 y1 x2 y2))
(append (line-intersection-pt trueline epi1line)
(line-intersection-pt trueline epi2line)))))
(when (>= relative-len relative-length-thresh)
(multiple-value-setq (pt1 pt2) (sloppy-points pt1 pt2 trueline endpoint-slop))
(let ((zvalue1 (cr-reference-value cr-reference-line (pt-vec-to-line pt1 vp)))
(zvalue2 (cr-reference-value cr-reference-line (pt-vec-to-line pt2 vp))))
(multiple-value-bind (lowheight highheight)
(vote-for-height-range
(height-cross-ratio 1overcamz lowz highz zvalue1)
(height-cross-ratio 1overcamz lowz highz zvalue2)
*1overcamz* *lowz* *highz*
:weight relative-len
:histogram histogram)
(push (isr2:copy-handle tok) *line-matches*)
(when prompt
(format t "Token ~s votes ~d for height range (~d,~d)~%"
tok relative-len lowheight highheight)))
; (display-line w (car pt1) (cadr pt1) (car pt2) (cadr pt2) numrows
; :color w:red :thickness 3)
(display-line w x1 y1 x2 y2 numrows :color *search-color* :thickness *search-thickness*)
()))))))))))
(defun get-J-image-number (tok)
(let ((str (format nil "~a" tok)))
(let ((ind (+ 1 (position "J" str :test #'string-equal))))
(read-from-string (subseq str ind (+ 1 ind))))))
(defun save-matches-to-file (filename)
(with-open-file (file filename :direction :output)
(format file "matched line data from L-shaped building~%")
(dolist (item (reverse *line-matches*) t)
(if (listp item)
(progn
(format file "===============================~%")
(format file "1 ~d ~d ~d ~d~%"
(coerce (car item) 'single-float)
(coerce (cadr item) 'single-float)
(coerce (caddr item) 'single-float)
(coerce (fourth item) 'single-float)))
(format file "~d ~d ~d ~d ~d~%"
(get-J-image-number item)
(isr2:value (list item 'x1))
(isr2:value (list item 'y1))
(isr2:value (list item 'x2))
(isr2:value (list item 'y2)))))))
|#
(defvar *matching-lines* nil "used to collect matching lines")
(defun collect-match-candidates (w grid line-index low1 low2 high1 high2 numrows
&key (deltatheta .1) (relative-length-thresh .25))
(multiple-value-bind (lowline epi2line highline epi1line)
(clip-to-quad-init low1 low2 high2 high1)
(let* ((vp (la:unit-normal lowline highline))
(cos-theta-thresh (cos (abs deltatheta)))
(candidates nil) (line nil) (trueline nil) (relative-len nil))
(setf candidates (coarse-get-lines-in-quadrilateral grid low1 low2 high2 high1))
(epipolar-display w low1 low2 high1 high2 numrows :zoom t)
(display-line-tokenset w candidates :color *line-color* :thickness *line-thickness*)
(epipolar-display w low1 low2 high1 high2 numrows :zoom nil)
(isr2:for-every-token (tok candidates (x1 y1 x2 y2))
(multiple-value-bind (x1 y1 x2 y2 ok?)
(clip-to-quad (isr2:value x1) (isr2:value y1) (isr2:value x2) (isr2:value y2)
lowline epi2line highline epi1line)
(when ok?
(let ((pt1 (list x1 y1))
(pt2 (list x2 y2))
(midpt (list (/ (+ x1 x2) 2.0) (/ (+ y1 y2) 2.0))))
(setf line (endpoints-to-line pt1 pt2))
(setf trueline (pt-vec-to-line midpt vp))
(when (< cos-theta-thresh (abs (la:dot-product (firstn 2 line) (firstn 2 trueline))))
(setf relative-len
(/ (vp:line-length x1 y1 x2 y2)
(apply #'(lambda (x1 y1 x2 y2) (vp:line-length x1 y1 x2 y2))
(append (line-intersection-pt trueline epi1line)
(line-intersection-pt trueline epi2line)))))
(when (>= relative-len relative-length-thresh)
(push (list line-index (isr2:copy-handle tok))
*matching-lines* )
(display-line w x1 y1 x2 y2 numrows :color *search-color* :thickness *search-thickness*)
())))))))))
(defun epipolar-collection-area (dlt1-inverse dlt2 x1 y1 x2 y2 z-range endpoint-slop)
(let* ((low1 (apply #'dlt-3d-to-2d dlt2 (backproject-point dlt1-inverse x1 y1 (car z-range))))
(low2 (apply #'dlt-3d-to-2d dlt2 (backproject-point dlt1-inverse x2 y2 (car z-range))))
(high1 (apply #'dlt-3d-to-2d dlt2 (backproject-point dlt1-inverse x1 y1 (cadr z-range))))
(high2 (apply #'dlt-3d-to-2d dlt2 (backproject-point dlt1-inverse x2 y2 (cadr z-range))))
(lowline (endpoints-to-line low1 low2))
(highline (endpoints-to-line high1 high2))
(slop (if (minusp (la:dot-product lowline (pt-to-nvec (car high1) (cadr high1))))
(- (abs endpoint-slop))
(abs endpoint-slop)))
(sloplowx (* (car lowline) (- slop)))
(sloplowy (* (cadr lowline) (- slop)))
(slophighx (* (car highline) slop))
(slophighy (* (cadr highline) slop)))
(list
(list (+ (car low1) sloplowx) (+ (cadr low1) sloplowy))
(list (+ (car low2) sloplowx) (+ (cadr low2) sloplowy))
(list (+ (car high1) slophighx) (+ (cadr high1) slophighy))
(list (+ (car high2) slophighx) (+ (cadr high2) slophighy))
low1 low2 high1 high2)))
(defun collect-lines-from-epipolar-area (w dlt1-inverse dlt2 line-index x1 y1 x2 y2 z-range grid numrows
&key (deltatheta .1) (relative-length-thresh .25)
(endpoint-slop 1) (prompt t))
(multiple-value-bind (low1 low2 high1 high2 l1 l2 h1 h2)
(values-list (epipolar-collection-area dlt1-inverse dlt2 x1 y1 x2 y2 z-range endpoint-slop))
(when prompt
(epipolar-display w l1 l2 h1 h2 numrows :zoom t :color w:red :thickness 1)
(epipolar-display w low1 low2 high1 high2 numrows)
(yes-or-no-p "continue"))
(collect-match-candidates
w grid line-index low1 low2 high1 high2 numrows
:deltatheta deltatheta
:relative-length-thresh relative-length-thresh)))
(defun collect-matching-lines (linelist z-range &key (endpoint-slop 1.0)(delta-theta .1)
(prompt t) (prompt-first nil))
(let ((w *display1*)
(w2 *display2*)
(first t)
(line-index 0))
(setf *matching-lines* nil)
(send *plot2* :clear)
(dolist (line linelist)
(incf line-index)
(multiple-value-bind (x1 y1 x2 y2) (values-list line)
(send w :restore-display 'j3lines)
(display-line w x1 y1 x2 y2 *j3numrows* :color *search-color* :thickness (1+ *search-thickness*))
(mapcar
#'(lambda (tks displayname dlt grid numrows)
(let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
(numcols (isr2:value (isr2:handle (list tks 'numcols))))
(label (isr2:value (isr2:handle (list tks 'label)))))
(set-label w2 label)
(send w2 :set-image-window 0 0 numcols numrows))
(when prompt
(send w2 :restore-display displayname))
(collect-lines-from-epipolar-area
w2 *j3inverse* dlt
line-index x1 y1 x2 y2 z-range
grid numrows
:deltatheta delta-theta :relative-length-thresh .1
:endpoint-slop endpoint-slop :prompt prompt)
(when (and first prompt-first)
(yes-or-no-p "continue"))
(setf first nil)
(when prompt (yes-or-no-p "next image")))
(list 'j1-data-lines 'j2-data-lines 'j3-data-lines 'j4-data-lines
'j5-data-lines 'j6-data-lines 'j7-data-lines 'j8-data-lines)
(list 'j1lines 'j2lines 'j3lines 'j4lines 'j5lines 'j6lines 'j7lines 'j8lines)
(list *j1dlt* *j2dlt* *j3dlt* *j4dlt* *j5dlt* *j6dlt* *j7dlt* *j8dlt*)
(list *j1grid* *j2grid* *j3grid* *j4grid* *j5grid* *j6grid* *j7grid* *j8grid*)
(list *j1numrows* *j2numrows* *j3numrows* *j4numrows*
*j5numrows* *j6numrows* *j7numrows* *j8numrows*))))
*matching-lines*))
(defun j8-collect-matching-lines (linelist z-range &key (endpoint-slop 1.0)(delta-theta .1)
(prompt t) (prompt-first nil))
(let ((w *display1*)
(w2 *display2*)
(first t)
(line-index 0))
(setf *matching-lines* nil)
(send *plot2* :clear)
(dolist (line linelist)
(incf line-index)
(multiple-value-bind (x1 y1 x2 y2) (values-list line)
(send w :restore-display 'j8lines)
(display-line w x1 y1 x2 y2 *j8numrows* :color *search-color* :thickness (1+ *search-thickness*))
(mapcar
#'(lambda (tks displayname dlt grid numrows)
(let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
(numcols (isr2:value (isr2:handle (list tks 'numcols))))
(label (isr2:value (isr2:handle (list tks 'label)))))
(set-label w2 label)
(send w2 :set-image-window 0 0 numcols numrows))
(when prompt
(send w2 :restore-display displayname))
(collect-lines-from-epipolar-area
w2 *j8inverse* dlt
line-index x1 y1 x2 y2 z-range
grid numrows
:deltatheta delta-theta :relative-length-thresh .1
:endpoint-slop endpoint-slop :prompt prompt)
(when (and first prompt-first)
(yes-or-no-p "continue"))
(setf first nil)
(when prompt (yes-or-no-p "next image")))
(list 'j1-data-lines 'j2-data-lines 'j3-data-lines 'j4-data-lines
'j5-data-lines 'j6-data-lines 'j7-data-lines 'j8-data-lines)
(list 'j1lines 'j2lines 'j3lines 'j4lines 'j5lines 'j6lines 'j7lines 'j8lines)
(list *j1dlt* *j2dlt* *j3dlt* *j4dlt* *j5dlt* *j6dlt* *j7dlt* *j8dlt*)
(list *j1grid* *j2grid* *j3grid* *j4grid* *j5grid* *j6grid* *j7grid* *j8grid*)
(list *j1numrows* *j2numrows* *j3numrows* *j4numrows*
*j5numrows* *j6numrows* *j7numrows* *j8numrows*))))
*matching-lines*))
(defun j6-collect-matching-lines (linelist z-range &key (endpoint-slop 1.0)(delta-theta .1)
(prompt t) (prompt-first nil))
(let ((w *display1*)
(w2 *display2*)
(first t)
(line-index 0))
(setf *matching-lines* nil)
(send *plot2* :clear)
(dolist (line linelist)
(incf line-index)
(multiple-value-bind (x1 y1 x2 y2) (values-list line)
(send w :restore-display 'j6lines)
(display-line w x1 y1 x2 y2 *j6numrows* :color *search-color* :thickness (1+ *search-thickness*))
(mapcar
#'(lambda (tks displayname dlt grid numrows)
(let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
(numcols (isr2:value (isr2:handle (list tks 'numcols))))
(label (isr2:value (isr2:handle (list tks 'label)))))
(set-label w2 label)
(send w2 :set-image-window 0 0 numcols numrows))
(when prompt
(send w2 :restore-display displayname))
(collect-lines-from-epipolar-area
w2 *j6inverse* dlt
line-index x1 y1 x2 y2 z-range
grid numrows
:deltatheta delta-theta :relative-length-thresh .1
:endpoint-slop endpoint-slop :prompt prompt)
(when (and first prompt-first)
(yes-or-no-p "continue"))
(setf first nil)
(when prompt (yes-or-no-p "next image")))
(list 'j1-data-lines 'j2-data-lines 'j3-data-lines 'j4-data-lines
'j5-data-lines 'j6-data-lines 'j7-data-lines 'j8-data-lines)
(list 'j1lines 'j2lines 'j3lines 'j4lines 'j5lines 'j6lines 'j7lines 'j8lines)
(list *j1dlt* *j2dlt* *j3dlt* *j4dlt* *j5dlt* *j6dlt* *j7dlt* *j8dlt*)
(list *j1grid* *j2grid* *j3grid* *j4grid* *j5grid* *j6grid* *j7grid* *j8grid*)
(list *j1numrows* *j2numrows* *j3numrows* *j4numrows*
*j5numrows* *j6numrows* *j7numrows* *j8numrows*))))
*matching-lines*))
(defun j8-test-all-epipolar-search-areas (linelist &key (endpoint-slop 1.0)(delta-theta .1)
(prompt t) (prompt-first nil))
(let ((w *display1*)
(w2 *display2*)
(first t))
(clear-height-histogram *accum-histogram*)
(send *plot1* :clear)
(send *plot2* :clear)
(dolist (line linelist)
(multiple-value-bind (x1 y1 x2 y2) (values-list line)
(send w :restore-display 'j8lines)
(display-line w x1 y1 x2 y2 *j8numrows* :color *search-color* :thickness (1+ *search-thickness*))
(mapcar
#'(lambda (tks displayname dlt grid numrows)
(let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
(numcols (isr2:value (isr2:handle (list tks 'numcols))))
(label (isr2:value (isr2:handle (list tks 'label)))))
(set-label w2 label)
(send w2 :set-image-window 0 0 numcols numrows))
(when prompt
(send w2 :restore-display displayname))
(clear-height-histogram *current-histogram*)
(test-epipolar-search-area w2 *j8inverse* dlt x1 y1 x2 y2 grid numrows
:deltatheta delta-theta :relative-length-thresh .1
:endpoint-slop endpoint-slop :prompt prompt)
(plot-histogram *plot2* *current-histogram* :max 1)
(add-to-height-histogram *accum-histogram* *current-histogram*)
(plot-histogram *plot1* *accum-histogram* :max 3)
(when (and first prompt-first)
(yes-or-no-p "continue"))
(setf first nil)
(when prompt
(do ((done (yes-or-no-p "next image") (yes-or-no-p "next image")))
(done t)
(print "enter line coordinate list (x1 y1 x2 y2)")
(let ((testline (eval (read))))
(test-epipolar-search-area w2 *j8inverse* dlt x1 y1 x2 y2 grid numrows
:deltatheta delta-theta :relative-length-thresh .1
:testline testline
:endpoint-slop endpoint-slop)))))
(list 'j1-data-lines 'j2-data-lines 'j3-data-lines 'j4-data-lines
'j5-data-lines 'j6-data-lines 'j7-data-lines )
(list 'j1lines 'j2lines 'j3lines 'j4lines 'j5lines 'j6lines 'j7lines )
(list *j1dlt* *j2dlt* *j3dlt* *j4dlt* *j5dlt* *j6dlt* *j7dlt*)
(list *j1grid* *j2grid* *j3grid* *j4grid* *j5grid* *j6grid* *j7grid*)
(list *j1numrows* *j2numrows* *j3numrows* *j4numrows* *j5numrows* *j6numrows* *j7numrows*))))
*accum-histogram*))
(defun j6-test-all-epipolar-search-areas (linelist &key (endpoint-slop 1.0)(delta-theta .1)
(prompt t) (prompt-first nil))
(let ((w *display1*)
(w2 *display2*)
(first t))
(clear-height-histogram *accum-histogram*)
(send *plot1* :clear)
(send *plot2* :clear)
(dolist (line linelist)
(multiple-value-bind (x1 y1 x2 y2) (values-list line)
(send w :restore-display 'j6lines)
(display-line w x1 y1 x2 y2 *j6numrows* :color *search-color* :thickness (1+ *search-thickness*))
(mapcar
#'(lambda (tks displayname dlt grid numrows)
(let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
(numcols (isr2:value (isr2:handle (list tks 'numcols))))
(label (isr2:value (isr2:handle (list tks 'label)))))
(set-label w2 label)
(send w2 :set-image-window 0 0 numcols numrows))
(when prompt
(send w2 :restore-display displayname))
(clear-height-histogram *current-histogram*)
(test-epipolar-search-area w2 *j6inverse* dlt x1 y1 x2 y2 grid numrows
:deltatheta delta-theta :relative-length-thresh .1
:endpoint-slop endpoint-slop :prompt prompt)
(plot-histogram *plot2* *current-histogram* :max 1)
(add-to-height-histogram *accum-histogram* *current-histogram*)
(plot-histogram *plot1* *accum-histogram* :max 3)
(when (and first prompt-first)
(yes-or-no-p "continue"))
(setf first nil)
(when prompt
(do ((done (yes-or-no-p "next image") (yes-or-no-p "next image")))
(done t)
(print "enter line coordinate list (x1 y1 x2 y2)")
(let ((testline (eval (read))))
(test-epipolar-search-area w2 *j6inverse* dlt x1 y1 x2 y2 grid numrows
:deltatheta delta-theta :relative-length-thresh .1
:testline testline
:endpoint-slop endpoint-slop)))))
(list 'j1-data-lines 'j2-data-lines 'j3-data-lines 'j4-data-lines
'j5-data-lines 'j7-data-lines 'j8-data-lines )
(list 'j1lines 'j2lines 'j3lines 'j4lines 'j5lines 'j7lines 'j8lines )
(list *j1dlt* *j2dlt* *j3dlt* *j4dlt* *j5dlt* *j7dlt* *j8dlt*)
(list *j1grid* *j2grid* *j3grid* *j4grid* *j5grid* *j7grid* *j8grid*)
(list *j1numrows* *j2numrows* *j3numrows* *j4numrows* *j5numrows* *j7numrows* *j8numrows*))))
*accum-histogram*))
(defun get-J-image-number (tok)
(let ((str (format nil "~a" tok)))
(let ((ind (+ 1 (position "J" str :test #'string-equal))))
(read-from-string (subseq str ind (+ 1 ind))))))
(defun get-all-J-matches (image-number)
(mapcan #'(lambda (match-pair)
(when (eq (get-J-image-number (cadr match-pair)) image-number)
(list match-pair)))
*matching-lines*))
(defvar *dlt-list* (list *j1dlt* *j2dlt* *j3dlt* *j4dlt* *j5dlt* *j6dlt* *j7dlt* *j8dlt*))
(defun save-matches-to-file (linelist filename &optional (numimages 8) &aux numlines)
(setf numlines (length linelist))
(with-open-file (file filename :direction :output)
(format file "~d~%" numlines) ;number of polygon lines
(format file "~d~%" numlines) ;perpendicularity constraints
(dotimes (i numlines)
(format file "~d ~d 0.0~%"
(+ 1 i)
(if (= i (- numlines 1))
1
(+ 2 i))))
(format file "~d~%" numlines) ;horizontal line constraints
(dotimes (i numlines)
(format file "~d~%" (+ 1 i)))
(format file "~d~%" numimages)
(dotimes (image numimages filename) ;DLT matrix for this image
(dotimes (r 3)
(dotimes (c 4)
(unless (= (+ r c) 5)
(format file "~f " (aref (elt *dlt-list* image) r c))))
(format file "~%"))
(let ((matches (get-all-j-matches (+ image 1))))
(format file "~d~%" (length matches))
(dolist (match (reverse matches) nil)
(let ((index (car match))
(tok (cadr match)))
(format file "~d ~d ~d ~d ~d~%" ;write one line match
index
(isr2:value (list tok 'x1))
(isr2:value (list tok 'y1))
(isr2:value (list tok 'x2))
(isr2:value (list tok 'y2)))))))))
(defun redisplay-lines (w tks displayname)
(let ((numrows (isr2:value (isr2:handle (list tks 'numrows))))
(numcols (isr2:value (isr2:handle (list tks 'numcols))))
(label (isr2:value (isr2:handle (list tks 'label)))))
(set-label w label)
(send w :set-image-window 0 0 numcols numrows))
(send w :restore-display displayname))