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
/
episearch.lisp
< prev
next >
Wrap
Lisp/Scheme
|
1996-03-06
|
19KB
|
497 lines
;; EPISEARCH.LISP
;;;
;;; Forming and searching epipolar regions for compatible lines
;;;
;;; Author: Robert T. Collins
;;; Date: Mar 1, 1995
;;; based on my earlier epipolar.lisp, written Dec 28, 1993
;;;
;-----------------------------------------------------------------
; (c) Copyright 1995 by The University of Massachusetts
;------------------------------------------------------------------
(in-package 'epipolar :nicknames '(epi))
;;;;****************************************************************
;;;; READING LINE DATA AND FORMING SPATIAL ACCESS GRIDS
(defun read-raw-data-lines-rc (tksname numrows numcols label filename
&optional (directory "/home/vidi/vis_radius_common/Boldt/"))
" Read an ascii file of data lines in row1 col1 row2 col2 contrast
format, and create an isr2 tokenset in x1 y1 x2 y2 contrast format.
Numrows and numcols specify the pixel height and width of the image."
(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)
(contrast "" :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))
(contrast (read file)))
(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))
(setf (isr2:value (list newtok 'contrast))
(coerce contrast 'single-float)))))))
(defun compute-line-tokenset-bounding-box (tksname)
(let ((minx 99999999.9) (miny 99999999.9)
(maxx -99999999.9) (maxy -99999999.9))
(isr2::for-every-token (ignore tksname (x1 y1 x2 y2))
(let ((vx1 (isr2::value x1)) (vy1 (isr2::value y1))
(vx2 (isr2::value x2)) (vy2 (isr2::value y2)))
(setf minx (min minx vx1 vx2))
(setf miny (min miny vy1 vy2))
(setf maxx (max maxx vx1 vx2))
(setf maxy (max maxy vy1 vy2))))
(values minx miny maxx maxy)))
(defun gridify-data-lines (tksname &key (rowbuckets 10) (colbuckets 10))
"Store lines in a grid structure for fast spatial access."
;;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.
(multiple-value-bind (minx miny maxx maxy)
(compute-line-tokenset-bounding-box tksname)
(let* ((numcols (- maxx minx))
(numrows (- maxy miny))
(colsize (ceiling numcols colbuckets))
(rowsize (ceiling numrows rowbuckets))
(grid (isr2::make-grid
tksname
minx maxx colsize
miny maxy 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) nil)
(isr2::grid-store gss (isr2::copy-handle tok)))
grid)))
;;;;****************************************************************
;;;; CLIPPING LINES INSIDE 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)
(defsubst clip-to-quad-outcodes (x y top left bot right)
(let ((vec (list x y 1.0)))
(+
(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-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
&key (ignorecode1 0) (ignorecode2 0))
" 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)
(line (endpoints-to-line (list x1 y1) (list x2 y2))))
(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))))
(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))))))
|#
;;;;****************************************************************
;;;; SEARCHING FOR CANDIDATE EPIPOLAR MATCHES
(defun isr-coarse-get-lines-in-quad (grid pt1 pt2 pt3 pt4)
(let ((gss (isr2::grid-gss grid))
(linelist nil))
(isr2::grid-unselect gss)
(isr2::rasterize-polygon gss (list pt1 pt2 pt3 pt4))
(let ((candidates (isr2::grid-retrieve gss)))
(isr2:for-every-token (tok candidates (x1 y1 x2 y2))
(push (list (isr2:value x1) (isr2:value y1)
(isr2:value x2) (isr2:value y2))
linelist)))
linelist))
(defun epipolar-search-area (projection1 projection2 x1 y1 x2 y2
&optional (zlow *lowz*) (zmid *midz*) (zhigh *highz*))
"Form the boundaries of an epipolar search region for the given line segment."
(values (apply #'project-point projection2
(backproject-point projection1 x1 y1 zlow))
(apply #'project-point projection2
(backproject-point projection1 x2 y2 zlow))
(apply #'project-point projection2
(backproject-point projection1 x1 y1 zmid))
(apply #'project-point projection2
(backproject-point projection1 x2 y2 zmid))
(apply #'project-point projection2
(backproject-point projection1 x1 y1 zhigh))
(apply #'project-point projection2
(backproject-point projection1 x2 y2 zhigh))))
(defun epipolar-search-area-bbox (view1 view2 linelist)
(let ((coordlist (mapcan #'(lambda (line)
(multiple-value-list
(apply #'epipolar-search-area
(view-projection view1)
(view-projection view2)
line)))
linelist)))
(let ((minx (reduce #'min coordlist :key #'car))
(miny (reduce #'min coordlist :key #'cadr))
(maxx (reduce #'max coordlist :key #'car))
(maxy (reduce #'max coordlist :key #'cadr)))
(list minx miny maxx maxy))))
(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 cos-line-angle (nvec1 nvec2)
(abs (la:dot-product (list (car nvec1) (cadr nvec1))
(list (car nvec2) (cadr nvec2)))))
(defun line-length (x1 y1 x2 y2)
(la:vector-length (list (- x2 x1) (- y2 y1))))
;;based on terms of Taylor series approx to 1/Sqrt(1+4*d*d/l*l)
(defun adjusted-cos-threshold (length pixslop)
(let ((a (/ (* 4 pixslop pixslop) (* length length))))
(/ 1.0 (+ 1 (/ a 2.0)))))
;;;;======================================================================
(defun collect-line-candidates (view low1 low2 mid1 mid2 high1 high2
&key (deltatheta .1)
(relative-length-thresh .25)
(endpoint-slop 1)
(clip-to-zrange t)
(only-display-matches nil) (zoom t)
&allow-other-keys)
(when (> (la::vector-length (la::v- low1 high1)) 0.0000001)
(multiple-value-bind (lowline epi2line highline epi1line)
(clip-to-quad-init low1 low2 high2 high1)
(let* ((vp (la:unit-normal lowline highline))
(midline (endpoints-to-line mid1 mid2))
(candidates nil) (line nil) (candidate-list nil)
(trueline nil) (relative-len nil)
(window (view-window view))
(cos-theta-thresh (cos (abs deltatheta))))
(multiple-value-bind (sloplow1 slophigh2)
(sloppy-points low1 high2 midline endpoint-slop)
(multiple-value-bind (sloplow2 slophigh1)
(sloppy-points low2 high1 midline endpoint-slop)
(multiple-value-bind (sloplowline slopepi2line slophighline slopepi1line)
(clip-to-quad-init sloplow1 sloplow2 slophigh2 slophigh1)
(setf candidates
(if *topdown-line-finder*
(cme::find-oriented-2dworld-lines
(view-topdown-edgels view)
(/ (* (atan (- (cadr mid1) (cadr mid2))
(- (car mid1) (car mid2)))
180.0) pi)
(reduce #'min (list low1 low2 high1 high2) :key #'car)
(reduce #'min (list low1 low2 high1 high2) :key #'cadr)
(reduce #'max (list low1 low2 high1 high2) :key #'car)
(reduce #'max (list low1 low2 high1 high2) :key #'cadr))
(isr-coarse-get-lines-in-quad (view-line-grid view)
sloplow1 sloplow2 slophigh2 slophigh1)))
(when (and *demo-mode* window (not only-display-matches))
(epipolar-display window sloplow1 sloplow2
slophigh1 slophigh2 :zoom zoom))
(setf *candidates* candidates)
(dolist (endpts candidates)
(multiple-value-bind (x1 y1 x2 y2) (values-list endpts)
(multiple-value-bind (clx1 cly1 clx2 cly2 ok?)
(clip-to-quad x1 y1 x2 y2 sloplowline slopepi2line
slophighline slopepi1line)
(when ok?
(when (and *demo-mode* window (not only-display-matches))
(display-line window x1 y1 x2 y2 :color *line-color*
:thickness *line-thickness*))
(when (not clip-to-zrange)
(multiple-value-setq (clx1 cly1 clx2 cly2)
(clip-to-quad x1 y1 x2 y2 sloplowline slopepi2line
slophighline slopepi1line
:ignorecode1 (+ topclipcode botclipcode)
:ignorecode2 (+ topclipcode botclipcode))))
(let ((pt1 (list clx1 cly1))
(pt2 (list clx2 cly2))
(midpt (list (/ (+ clx1 clx2) 2.0) (/ (+ cly1 cly2) 2.0))))
(setf line (endpoints-to-line pt1 pt2))
(setf trueline (pt-vec-to-line midpt vp))
(when *debug-mode*
(format t "Candidate (~,2f ~,2f ~,2f ~,2f)"
(car pt1) (cadr pt1) (car pt2) (cadr pt2))
(format t " cos test ~,3f < ~,3f~%"
cos-theta-thresh (cos-line-angle line trueline)))
(when (< cos-theta-thresh (cos-line-angle line trueline))
(setf relative-len
(min 1.0
(/ (line-length clx1 cly1 clx2 cly2)
(apply #'line-length
(append
(line-intersection-pt trueline epi1line)
(line-intersection-pt trueline epi2line))))))
(when *debug-mode*
(format t " relative len test ~,3f >= ~,3f~%"
relative-len relative-length-thresh))
(when (>= relative-len relative-length-thresh)
(when (and *demo-mode* window)
(display-line window clx1 cly1 clx2 cly2
:color *match-color*
:thickness *match-thickness*))
(push (list (list pt1 pt2) trueline relative-len)
candidate-list))))))))
candidate-list)))))))
(defun histogram-line-candidates (view1 u1 v1 u2 v2 view2
&key (histogram *current-histogram*)
(deltatheta .1)
(relative-length-thresh .25)
(peak-weight? nil)
(lowz *lowz*) (midz *midz*)(highz *highz*)
(endpoint-slop 1)
(zoom t))
(multiple-value-bind (low1 low2 mid1 mid2 high1 high2)
(epipolar-search-area (view-projection view1) (view-projection view2)
u1 v1 u2 v2 lowz midz highz)
(when (> (la::vector-length (la::v- low1 high1)) 0.0000001)
(multiple-value-bind (lowline epi2line highline epi1line)
(clip-to-quad-init low1 low2 high2 high1)
(let* ((vp (la:unit-normal lowline highline))
(midline (endpoints-to-line mid1 mid2))
(cr-reference-line
(cr-reference-line lowline epi2line highline epi1line))
(lowcr (cr-reference-value cr-reference-line lowline))
(midcr (cr-reference-value cr-reference-line midline))
(highcr (cr-reference-value cr-reference-line highline))
(candidate-list nil)
(expected-len (apply #'line-length
(append
(line-intersection-pt midline epi1line)
(line-intersection-pt midline epi2line))))
(deltatheta (max deltatheta
(min (atan (/ (* 2.0 endpoint-slop)
expected-len))
(* 3.0 deltatheta)))))
(setf candidate-list
(collect-line-candidates view2
low1 low2 mid1 mid2 high1 high2
:deltatheta deltatheta :endpoint-slop 0
:relative-length-thresh relative-length-thresh
:zoom zoom))
(dolist (candidate candidate-list histogram)
(let ((pt1 (car (car candidate)))
(pt2 (cadr (car candidate)))
(trueline (second candidate))
(relative-len (third candidate)))
(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-invariant lowcr midcr highcr zvalue1)
(height-invariant lowcr midcr highcr zvalue2)
lowz midz highz
:weight relative-len
:peak-weight? peak-weight?
:histogram histogram)
(when *debug-mode*
(format t "Line (~,2f ~,2f ~,2f ~,2f)"
(car pt1) (cadr pt1) (car pt2) (cadr pt2))
(format t " votes ~,3f for height range (~,2f,~,2f)~%"
relative-len lowheight highheight)))))
))))))
(defun histogram-linelist-candidates (view1 linelist view2 &rest keys)
(let ((firstline nil)
(window (view-window view2)))
(when (and *demo-mode* window)
(let ((boundingbox (epipolar-search-area-bbox view1 view2 linelist)))
(setf firstline t)
(activate-window window)
(apply #'cme::zoom-to-bounding-box window boundingbox)))
(dolist (line linelist)
(when (and *demo-mode* window (not firstline))
(activate-window window :refresh t))
(apply #'histogram-line-candidates
view1 (first line) (second line) (third line) (fourth line)
view2
:zoom nil
keys)
(when (and *demo-mode* (eq *pause-mode* :lines))
(setf firstline nil)
;;; (plot-histogram *plot1* *accum-histogram* :max 3)
(synch-epipolar-screen window)
(setf *pause-mode*
(if (yes-or-no-p "Pause between lines?") :lines :views))))))
(defun collect-matches (view1 u1 v1 u2 v2 view2
&rest keys
&key (lowz *lowz*) (midz *midz*) (highz *highz*)
&allow-other-keys)
(multiple-value-bind (low1 low2 mid1 mid2 high1 high2)
(epipolar-search-area
(view-projection view1)
(view-projection view2)
u1 v1 u2 v2 lowz midz highz)
(let ((match-list (apply #'collect-line-candidates view2
low1 low2 mid1 mid2 high1 high2
:clip-to-zrange nil
:zoom nil
keys)))
(mapcar #'car match-list))))
(defun collect-linelist-matches (view1 linelist view2 &rest keys)
(let ((window (view-window view2)))
(when (and *demo-mode* window)
(activate-window window)
(let ((boundingbox (epipolar-search-area-bbox view1 view2 linelist)))
(apply #'cme::zoom-to-bounding-box window boundingbox)))
(let ((match-list nil))
(dotimes (i (length linelist))
(let* ((line (elt linelist i))
(matches
(apply #'collect-matches
view1
(first line) (second line) (third line) (fourth line)
view2
keys)))
(when matches
(push (list (+ i 1) matches) match-list))))
match-list)))