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 / epiproj-matrix.lisp < prev    next >
Lisp/Scheme  |  1995-07-20  |  3KB  |  78 lines
  1. ;;; EPIPROJ-MATRIX.LISP
  2. ;;;
  3. ;;; Forward and backward projection using 3x4 projection matrices
  4. ;;;
  5. ;;; Author: Robert T. Collins
  6. ;;; Date: Mar 1, 1995
  7. ;;; based on my earlier epipolar.lisp, written Dec 28, 1993
  8. ;;;
  9. ;-----------------------------------------------------------------
  10. ; (c) Copyright 1995 by The University of Massachusetts
  11. ;------------------------------------------------------------------
  12.  
  13. (in-package 'epipolar :nicknames '(epi))
  14.  
  15.  
  16. ;;;;****************************************************************
  17. ;;;; MANIPULATING 3x4 PROJECTION MATRICES
  18.  
  19. (defun projmat-to-amat-bvec (projmat)
  20.   "  Break a 3x4 projection matrix into a 3x3 matrix containing the
  21.   first three columns and a 3x1 vector containing the last column."
  22.   (values
  23.    (make-array '(3 3) :initial-contents 
  24.        (list (list (aref projmat 0 0) (aref projmat 0 1) (aref projmat 0 2))
  25.          (list (aref projmat 1 0) (aref projmat 1 1) (aref projmat 1 2))
  26.          (list (aref projmat 2 0) (aref projmat 2 1) (aref projmat 2 2))))
  27.    (list (aref projmat 0 3) (aref projmat 1 3) (aref projmat 2 3))))
  28.  
  29. (defun amat-bvec-to-projmat (amat bvec)
  30.   "  Concatenate a 3x3 matrix and a 3x1 vector into a 3x4 projection matrix."
  31.   (make-array '(3 4) 
  32.     :initial-contents 
  33.     (list
  34.       (list (aref amat 0 0) (aref amat 0 1) (aref amat 0 2) (elt bvec 0))
  35.       (list (aref amat 1 0) (aref amat 1 1) (aref amat 1 2) (elt bvec 1))
  36.       (list (aref amat 2 0) (aref amat 2 1) (aref amat 2 2) (elt bvec 2)))))
  37.  
  38. (defun inverse-projmat (projmat)
  39.   (multiple-value-bind (amat bvec) (projmat-to-amat-bvec projmat)
  40.     (let ((invamat (la:inverse3 amat)))
  41.       (amat-bvec-to-projmat invamat (la:vs -1.0 (la:m* invamat bvec))))))
  42.  
  43. (defun projmat-3d-to-2d (projmat x y z)
  44.   "  Projects 3D world point x y z into image plane using the given
  45.   3x4 projective projective transformation matrix."
  46.   (nvec-to-pt (la::listarray (la:m* projmat (list x y z 1.0)))))
  47.  
  48. (defun projmat-2d-to-3d (projmat x y
  49.         &optional (projmat-inv (inverse-projmat projmat)))
  50.   "  Backprojects 2d image point x y into a world ray using inverse of
  51.   the given 3x4 projective transformation matrix. The resulting world
  52.   ray is returned as two vectors a and b describing the line k a + b."
  53.   (values (la::listarray (la:m* projmat-inv (list x y 1.0 0.0)))
  54.       (la::listarray (la:m* projmat-inv (list 0.0 0.0 0.0 1.0)))))
  55.  
  56.  
  57. ;;;;****************************************************************
  58. ;;;; GENERIC PROJECTION FUNCTIONS INTERFACED BY THE EPIPOLAR MATCHER
  59.  
  60. ;;; here projection is a structure of type projection
  61.  
  62. (defun project-point (projection x y z)
  63.   "Projects 3D world point x y z into image plane u v."
  64.   (projmat-3d-to-2d (projection-projmat projection) x y z))
  65.  
  66. (defun backproject-point (projection u v zval)
  67.   "Backproject image point u v and return the 3d point where
  68.   the backprojection ray intersects the plane z = zval."
  69.   (when (null (projection-inverse-projmat projection))
  70.      (setf (projection-inverse-projmat projection)
  71.        (inverse-projmat (projection-projmat projection))))
  72.   (let ((invproj (projection-inverse-projmat projection)))
  73.     (multiple-value-bind (a b)
  74.       (projmat-2d-to-3d nil u v invproj)
  75.       (let ((k (/ (- zval (third b)) (third a))))
  76.     (la:v+ b (la:vs k a))))))
  77.  
  78.