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
/
epiproj-matrix.lisp
< prev
next >
Wrap
Lisp/Scheme
|
1995-07-20
|
3KB
|
78 lines
;;; EPIPROJ-MATRIX.LISP
;;;
;;; Forward and backward projection using 3x4 projection matrices
;;;
;;; 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))
;;;;****************************************************************
;;;; MANIPULATING 3x4 PROJECTION MATRICES
(defun projmat-to-amat-bvec (projmat)
" Break a 3x4 projection matrix into a 3x3 matrix containing the
first three columns and a 3x1 vector containing the last column."
(values
(make-array '(3 3) :initial-contents
(list (list (aref projmat 0 0) (aref projmat 0 1) (aref projmat 0 2))
(list (aref projmat 1 0) (aref projmat 1 1) (aref projmat 1 2))
(list (aref projmat 2 0) (aref projmat 2 1) (aref projmat 2 2))))
(list (aref projmat 0 3) (aref projmat 1 3) (aref projmat 2 3))))
(defun amat-bvec-to-projmat (amat bvec)
" Concatenate a 3x3 matrix and a 3x1 vector into a 3x4 projection matrix."
(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-projmat (projmat)
(multiple-value-bind (amat bvec) (projmat-to-amat-bvec projmat)
(let ((invamat (la:inverse3 amat)))
(amat-bvec-to-projmat invamat (la:vs -1.0 (la:m* invamat bvec))))))
(defun projmat-3d-to-2d (projmat x y z)
" Projects 3D world point x y z into image plane using the given
3x4 projective projective transformation matrix."
(nvec-to-pt (la::listarray (la:m* projmat (list x y z 1.0)))))
(defun projmat-2d-to-3d (projmat x y
&optional (projmat-inv (inverse-projmat projmat)))
" Backprojects 2d image point x y into a world ray using inverse of
the given 3x4 projective transformation matrix. The resulting world
ray is returned as two vectors a and b describing the line k a + b."
(values (la::listarray (la:m* projmat-inv (list x y 1.0 0.0)))
(la::listarray (la:m* projmat-inv (list 0.0 0.0 0.0 1.0)))))
;;;;****************************************************************
;;;; GENERIC PROJECTION FUNCTIONS INTERFACED BY THE EPIPOLAR MATCHER
;;; here projection is a structure of type projection
(defun project-point (projection x y z)
"Projects 3D world point x y z into image plane u v."
(projmat-3d-to-2d (projection-projmat projection) x y z))
(defun backproject-point (projection u v zval)
"Backproject image point u v and return the 3d point where
the backprojection ray intersects the plane z = zval."
(when (null (projection-inverse-projmat projection))
(setf (projection-inverse-projmat projection)
(inverse-projmat (projection-projmat projection))))
(let ((invproj (projection-inverse-projmat projection)))
(multiple-value-bind (a b)
(projmat-2d-to-3d nil u v invproj)
(let ((k (/ (- zval (third b)) (third a))))
(la:v+ b (la:vs k a))))))