;;; -*- Mode: Lisp; Syntax: Common-lisp; Package: MU; Base: 10. -*- ;;; Copyright (c) 1987, Massachusetts Institute of Technology ;;; Authors: Mike Drumheller, Walter E. Gillett ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ;;; ;;; This file contains code related to interpolation of sparse depth data, such as ;;; that produced by my stereo program. ;;; ;;; Some of the programs in this file are directly related to interpolation, like ;;; RUBBER-SHEET!! and THIN-PLATE!!. Other programs (later on in the file) have to ;;; do with simple statistical heuristics for "cleaning up" data before ;;; interpolating it. Still others are related to making contour maps. ;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; (in-package :mu) ;;; DIFFUSION-INNER-LOOP!! is the inner loop of DIFFUSE!!. This function does ;;; repeated convolution of IMAGE!! with a mask like this: 1 ;;; 1 0 1 ;;; 1 ;;; BOOLEAN-OF-FIXED-VALUES!! can be either NIL or a boolean-pvar. If it is NIL, ;;; then it has no effect; the program simple does the repeated averaging. If it is ;;; a boolean-pvar, then every processor containing T holds its value in IMAGE!! ;;; fixed throughout the computation, i.e., these values are boundary conditions ;;; that are *not* affected by smoothing. (*defunc DIFFUSION-INNER-LOOP!! (image!! iterations &optional boolean-of-fixed-values!!) (*let ((result!! image!!)) (declare (type (signed-pvar (pvar-length image!!)) result!!)) (*if (if boolean-of-fixed-values!! (not!! boolean-of-fixed-values!!) t!!) (dotimes (i iterations) (*set result!! (ash!! (+!! (get-grid!! result!! 1 0) (get-grid!! result!! 0 1) (get-grid!! result!! -1 0) (get-grid!! result!! 0 -1)) (!! -2))))) result!!)) ;;; DIFFUSE!! does repeated averaging of IMAGE!!. It does it ITERATIONS times. ;;; ASH-BEFORE-DIFFUSING is a number, like 8 or 10, that controls numerical ;;; precision. The data gets arithmetically shifted ASH-BEFORE-DIFFUSING bits ;;; before averaging, and after averaging it gets shifted back down (the down-shift ;;; is according ASH-AFTER-DIFFUSING, which defaults to the negative of ;;; ASH-BEFORE-DIFFUSING.) ;;; ;;; That way you can diffuse eight-bit INTEGER data with reasonable precision and ;;; convergence. I recommend several shift bits, like ASH-BEFORE-DIFFUSING = 8. ;;; ;;; You need not smooth all of the data in IMAGE!!; you can regard some of it as ;;; "known" and therefore "fixed." There are two ways of controlling this. One way ;;; is to supply a pvar to the argument BOOLEAN-OF-FIXED-VALUES!!. If you do this, ;;; every processor where BOOLEAN-OF-FIXED-VALUES!! is T will hold its IMAGE!! value ;;; fixed, ie those IMAGE!! values will not be affected by the smoothing process. ;;; ;;; On the other hand, you could leave BOOLEAN-OF-FIXED-VALUES!! to be just NIL, and ;;; instead supply a T to the argument HOLD-NONZERO-INPUTS-FIXED?. In this case, ;;; the "boolean-of-fixed-values" is just (not!! (zerop!! image!!)). Confused now? ;;; Good! (*defunc DIFFUSE!! (image!! iterations &optional ash-before-diffusing hold-nonzero-inputs-fixed? boolean-of-fixed-values!! (ash-after-diffusing (- ash-before-diffusing))) (*let* ((ashed-image!! (ash!! image!! (!! ash-before-diffusing))) (diffused-ashed-image!! (cond (hold-nonzero-inputs-fixed? (diffusion-inner-loop!! ashed-image!! iterations (nzerop!! image!!))) (boolean-of-fixed-values!! (diffusion-inner-loop!! ashed-image!! iterations boolean-of-fixed-values!!)) (t (diffusion-inner-loop!! ashed-image!! iterations)))) (result!! (ash!! diffused-ashed-image!! (!! ash-after-diffusing)))) (declare (type (signed-pvar (+ (pvar-length image!!) ash-before-diffusing)) ashed-image!! diffused-ashed-image!!) (type (signed-pvar (+ ash-after-diffusing (pvar-length ashed-image!!))) result!!)) result!!)) ;;; This is just a renaming of DIFFUSE!!. (*defunc RUBBER-SHEET!! (image!! iterations &optional ash) (diffuse!! image!! iterations ash t nil)) ;;; THIN-PLATE-INNER-LOOP!! is the inner loop of the pseudo-useful function ;;; THIN-PLATE!! (see below). THIN-PLATE-INNER-LOOP!! performs repeated ;;; convolutions of the data with a special kernel (see below). The argument list ;;; can be decoded by looking at the documentation to DIFFUSE!! and ;;; DIFFUSION-INNER-LOOP!! (see above). ;;; ;;; Integers only, please. ;;; ;;; This program is used for interpolating between sparse data using a thin-plate model. It ;;; is based on the equilibrium equation for a simply-supported linear elastic thin plate: ;;; ;;; __ 4 ;;; \/ d = 0, ;;; ;;; __4 ;;; where d is the vertical displacement and \/ is the fourth-order linear symmetric ;;; differential operator (i.e., fourth-order "del"). ;;; __4 ;;; For a discrete regular orthogonal grid, \/ may be approximated by the following ;;; convolution kernel. ;;; ;;; 1/4 ;;; ;;; 1/2 -2 1/2 ;;; ;;; 1/4 -2 5 -2 1/4, ;;; ;;; 1/2 -2 1/2 ;;; ;;; 1/4 ;;; ;;; which is re-labelled for convenient documentation: ;;; ;;; a ;;; ;;; b c d ;;; ;;; e f g h i ;;; ;;; j k l ;;; ;;; m ;;; ;;; The basic approach is to repeatedly set each pixel to be the result of convolving with ;;; this kernel. As in the diffusion program above, certain input points may be held fixed. ;;; These are indicated by a 1 at location FIXED-POINTS. ;;; ;;; A note for keeping things straight in your head: On each iteration, the convolution ;;; with the above mask is calculated, divided by CONSTANT, and subtracted from the previous ;;; result. That give the new result. CONSTANT is voodoo, mark my word. You can think of ;;; it as a rate-controlling factor or a "damping constant." If ;;; CONSTANT is too small, ;;; the solution process is unstable and you get weird patterns. CONSTANT should therefore ;;; always be 8 or more. 8 seems to give the fastest converging solution, though it would ;;; probably be possible to use 8 for the first 20 iterations, then 5 for the next 100, then ;;; 1 for the next 1000, or something like that. Anyone know for sure? ;;; ;;; Another note: You may be familiar with the above differential equation and kernel, and ;;; if so you're probably wondering "Hmm, I've seen that kernel before, but it was ;;; normalized so that 20 appeared in the center, not 5." Well, I can't remember why I did ;;; that. --Mike. ;;; ;;; Final note: It is possible to write this program much more efficiently, I just don't ;;; have time to do it right now. See the old rel-2 version at the bottom of this file. (*defunc THIN-PLATE-INNER-LOOP!! (image!! iterations &optional boolean-of-fixed-values!! (constant 8)) (if (not (>= (round constant) 8)) (error "(round CONSTANT) is not greater than 8. You might as well put a gun to your head. (See the documentation for this routine.)")) (*let ((result!! image!!) (const!! (!! (round constant)))) (declare (type signed-pvar result!! const!!)) (*if (if boolean-of-fixed-values!! (not!! boolean-of-fixed-values!!) t!!) (dotimes (i iterations) (*set result!! (-!! result!! (/!! (+!! (ash!! (get-grid!! result!! 0 -2) (!! -2));<-Accumulate (1/4)a (ash!! (get-grid!! result!! -1 -1) (!! -1));<-Accumulate (1/2)b (-!! (ash!! (get-grid!! result!! 0 -1) (!! 1)));<-Accumulate (-2)c (ash!! (get-grid!! result!! 1 -1) (!! -1));<-Accumulate (1/2)d (ash!! (get-grid!! result!! -2 0) (!! -2));<-Accumulate (1/4)e (-!! (ash!! (get-grid!! result!! -1 0) (!! 1)));<-Accumulate (-2)f (ash!! result!! (!! 2));--Accumulate ( 5)g result!! ; / (-!! (ash!! (get-grid!! result!! 1 0) (!! 1)));<-Accumulate ( -2)h (ash!! (get-grid!! result!! 2 0) (!! -2));<-Accumulate (1/4)i (ash!! (get-grid!! result!! -1 1) (!! -1));<-Accumulate (1/2)j (-!! (ash!! (get-grid!! result!! 0 1) (!! 1)));<-Accumulate ( -2)k (ash!! (get-grid!! result!! 1 1) (!! -1));<-Accumulate (1/2)l (ash!! (get-grid!! result!! 0 2) (!! -2)));<-Accumulate (1/4)m const!!))))) result!!)) (*defunc THIN-PLATE!! (image!! iterations &optional (ash-before 8) (ash-after (- ash-before)) hold-nonzero-inputs-fixed? boolean-of-fixed-values) (*let (;; (ashed-image!! (ash!! image!! (!! ash-before))) (thin-plate-interpolated-ashed-image!! (cond (hold-nonzero-inputs-fixed? (thin-plate-inner-loop!! image!! iterations (nzerop!! image!!))) (boolean-of-fixed-values (thin-plate-inner-loop!! image!! iterations boolean-of-fixed-values)) (t (thin-plate-inner-loop!! image!! iterations))))) (ash!! thin-plate-interpolated-ashed-image!! (!! ash-after)))) ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ;;; These the main sparse-disparity-map interpolation routines used by the stereo ;;; demo. (*defunc DIFFUSION-INTERPOLATION!! (image!! &optional (constrained-iters 100) (free-iters 10) (ash-before-diff 8) (ash-after-diff (- ash-before-diff)) boolean-of-fixed-values!!) (diffuse!! (diffuse!! image!! constrained-iters ash-before-diff (not boolean-of-fixed-values!!) boolean-of-fixed-values!! 0) free-iters 0 nil nil ash-after-diff)) (*defunc THIN-PLATE-INTERPOLATION!! (image!! &optional (pre-thin-plate-diff-iters 500) (constrained-thin-plate-iters 100) (free-diff-iters 10) (ash-before 8) (ash-after (- ash-before))) (*let* ((fixed-values!! (nzerop!! image!!)) (pre-thin-plate!! (diffuse!! image!! pre-thin-plate-diff-iters ash-before nil fixed-values!! 0))) (diffuse!! (thin-plate!! pre-thin-plate!! constrained-thin-plate-iters 0 0 nil fixed-values!!) free-diff-iters 0 nil nil ash-after))) ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ;;; Contour-map drawing routines. ;;; TRANSITIONS!! is used for drawing contour maps. It returns a boolean-pvar that ;;; is T wherever the IMAGE!! changes value. Sort of a primitive edge detector. (*defunc TRANSITIONS!! (image!!) (if!! (or!! (not!! (=!! image!! (get-grid!! image!! 1 0))) (not!! (=!! image!! (get-grid!! image!! 0 1)))) t!! nil!!)) ;;; CONTOUR-MAP!! takes a pvar of numbers and returns a boolean-pvar containing a contour maps ;;; of the numbers. The idea is to build a version of 2-D function that has 1-bit transitions ;;; evenly divided NUMBER-OF-LEVEL-CURVES times between MIN and MAX. This insures that there ;;; are NUMBER-OF-LEVEL-CURVES level-curves divided between MIN and MAX. (Note that the ;;; function TRANSITIONS! is used here.) ;;; ;;; IMAGE!! is typically an interpolated disparity map. ;;; ;;; The four-expand is done to get rid of bad "checkerboard" patterns that sometimes appear in ;;; the output of diffusion operations. (*defunc CONTOUR-MAP!! (image!! min max number-of-level-curves &optional (ash-for-precision 8)) (let* ((float-ratio (/ (float number-of-level-curves) (1+ (- max min))))) (*let* ((r!! (!! (round (ash float-ratio ash-for-precision)))) ; <--Ashed for ; numerical precision. (adjusted-image!! (ash!! (*!! image!! r!!) (!! (- ash-for-precision))))) (*set adjusted-image!! (bash-pixels-with-more-than-N-brighter-or-dimmer-neighbors!! adjusted-image!! 4 :brighter)) (*set adjusted-image!! (bash-pixels-with-more-than-N-brighter-or-dimmer-neighbors!! adjusted-image!! 4 :dimmer)) (transitions!! adjusted-image!!)))) ;;; The purpose of FILL-BORDERS!! is obvious. This should be done before ;;; interpolating (*defunc FILL-BORDERS!! (image!! value left-width right-width top-width bottom-width) (*let ((my-x!! (x!!)) (my-y!! (y!!))) (if!! (or!! (!! my-x!! (!! (- (1- (dimension-size 0)) right-width))) (>!! my-y!! (!! (- (1- (dimension-size 1)) bottom-width)))) (!! value) image!!))) ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ;;; This section contains code for "cleaning up" disparity maps before interpolation. ;;; REMOVE-ISOLATED-PIXELS!! Removes pixels that have no nonzero neigbhors. Useful for ;;; cleaning up disparity maps. (*defunc REMOVE-ISOLATED-PIXELS!! (image!!) (*let* ((z!! (zerop!! image!!)) (remove-p!! (not!! z!!))) (loop for n from 0 to 7 do (*set remove-p!! (and!! remove-p!! (nearest-neighbor!! z!! n nil!!)))) (if!! remove-p!! (!! 0) image!!))) (*defunc REMOVE-ISOLATED-BOOLEANS!! (bool!!) (declare (type boolean-pvar bool!!)) (*let ((remove-p!! bool!!) (not-bool!! (not!! bool!!))) (declare (type boolean-pvar remove-p!! not-bool!!)) (loop for n from 0 to 7 do (*set remove-p!! (and!! remove-p!! (nearest-neighbor!! not-bool!! n nil!!)))) (if!! remove-p!! nil!! bool!!))) ;;; ***old code*** ;;; 11/8/87 WEG Made remove-p!! an optional parameter so that the caller can find out ;;; which pixels are removed. ;(*defunc REMOVE-ISOLATED-PIXELS!! (image!! &optional remove-p!!) ; (cmv::with-temp-pvars ; (let ((z!! (*lx::allocate-boolean-pvar :initial-value (zerop!! image!!))) ; (remove-p!! (if remove-p!! remove-p!! (*lx::allocate-boolean-pvar)))) ; (*set remove-p!! (not!! z!!)) ; (loop for n from 0 to 7 do ; (*set remove-p!! (and!! remove-p!! ; (nearest-neighbor!! z!! n nil!!)))) ; (if!! remove-p!! ; (!! 0) ; image!!)))) ;;; REMOVE-PIXELS-WITH-DIFFERENT-NEIGHBORS!! Removes pixels whose set of eight immediate ;;; neighbors are all either equal to each other or zero, but different from the center pixel. ;;; Useful for cleaning up disparity maps. (*defunc REMOVE-PIXELS-WITH-DIFFERENT-NEIGHBORS!! (image!! &optional (times 3)) (*let ((same-or-zero-p!! t!!) (scratch!! (!! 0))) (loop for i from 0 below times do (loop for n from 0 to 7 do (*let ((neighbor!! (nearest-neighbor!! image!! n))) (*if (zerop!! scratch!!) (*set scratch!! neighbor!!)) (*set same-or-zero-p!! (and!! same-or-zero-p!! (or!! (zerop!! neighbor!!) (=!! neighbor!! scratch!!)))))) (if!! (and!! same-or-zero-p!! (not!! (=!! image!! scratch!!))) (!! 0) image!!)))) ;;; SPARSE-IMAGE-MEAN!! Calculates, at each NONZERO pixel where ;;; BOOLEAN-OF-PIXELS-TO-CONSIDER!! is T, the mean of surrounding square patch of width WIDTH. ;;; Pixels where BOOLEAN-OF-PIXELS-TO-CONSIDER!! is NIL are not included in the average. The ;;; central pixel is not included in the average. ;;; ;;; Useful for calculating the mean value in a neighborhood of a sparse disparity ;;; map. (defun SPARSE-IMAGE-MEAN!! (image!! boolean-of-pixels-to-consider!! width) (if (not (and (integerp width) (>= width 3) (oddp width))) (error "For SPARSE-IMAGE-MEAN!, the argument WIDTH must be an odd integer greater than or equal to 3: ~a" width)) (*let ((sum!! (!! 0)) (count!! (!! 0))) (declare (type (signed-pvar (+ (pvar-length image!!) (integer-length (* width width)))) sum!!) (type (field-pvar (integer-length (* width width))) count!!)) (with-pvars-moving-in-rectangle ((temp image!!) (b boolean-of-pixels-to-consider!!)) x y width width (if (not (and (zerop x) (zerop y))) (*cond (b (*incf sum!! temp) (*incf count!!))))) (if!! (and!! (nzerop!! count!!) boolean-of-pixels-to-consider!!) (round!! (/!! sum!! count!!)) (!! 0)))) ;;; Calculates, at each NONZERO pixel where BOOLEAN-OF-PIXELS-TO-CONSIDER!! is T, the sum of ;;; the squares of the deviations of surrounding square patch of width WIDTH. Pixels where ;;; BOOLEAN-OF-PIXELS-TO-CONSIDER!! is NIL are not included in the variance. The central ;;; pixel is not included in the variance. (defun SPARSE-IMAGE-VARIANCE!! (image!! boolean-of-pixels-to-consider!! width &optional mean) (if (not (and (integerp width) (>= width 3) (oddp width))) (error "For SPARSE-FIELD-MEAN!, the argument WIDTH must be an odd integer greater than or equal to 3!")) (*let* ((m!! (if mean mean (sparse-image-mean!! image!! boolean-of-pixels-to-consider!! width))) (squared-devs!! (square!! (-!! image!! m!!))) (sum!! (!! 0)) (count!! (!! 0))) (declare (type (signed-pvar (pvar-length image!!)) m!!) (type (field-pvar (* 2 (pvar-length image!!))) squared-devs!!) (type (field-pvar (+ (integer-length (* width width)) (* 2 (pvar-length image!!)))) sum!!) (type (field-pvar (integer-length (* width width))) count!!)) (with-pvars-moving-in-rectangle ((temp squared-devs!!) (b boolean-of-pixels-to-consider!!)) x y width width (if (not (and (zerop x) (zerop y))) (*cond (b (*incf sum!! temp) (*incf count!!))))) (if!! (and!! (nzerop!! count!!) boolean-of-pixels-to-consider!!) (round!! (/!! sum!! count!!)) (!! 0)))) ;;; It is highly questionable whether this function works. ;;; ;;; Mike's comment above is overly pessimistic (the function appears to work), but it ;;; did require one fix: clear the boolean-of-pixels-to-consider at bad locations as ;;; well as the data itself. (WEG 11/8/87) (*defunc *CLEAR-IF-HIGH-VARIANCE-OR-IF-VERY-DIFFERENT-FROM-MEAN (image!! boolean-of-pixels-to-consider!! max-allowed-difference-from-mean max-allowed-variance &optional (width 9)) (*let* ((mean!! (sparse-image-mean!! image!! boolean-of-pixels-to-consider!! width)) (standard-dev!! (sparse-image-variance!! image!! boolean-of-pixels-to-consider!! width mean!!))) (declare (type (signed-pvar (pvar-length image!!)) mean!!) (type (field-pvar (+ (integer-length (* width width)) (* 2 (pvar-length image!!)))) standard-dev!!)) ; (if!! (and!! boolean-of-pixels-to-consider!! ; (or!! (>!! standard-dev!! (!! max-allowed-variance)) ; (>!! (abs!! (-!! image!! mean!!)) ; (!! max-allowed-difference-from-mean)))) ; (!! 0) ; image!!))) ;; drop the bogus pixels (*when (and!! boolean-of-pixels-to-consider!! (or!! (>!! standard-dev!! (!! max-allowed-variance)) (>!! (abs!! (-!! image!! mean!!)) (!! max-allowed-difference-from-mean)))) (*set boolean-of-pixels-to-consider!! nil!!)) (*set image!! (if!! boolean-of-pixels-to-consider!! image!! (!! 0)))))