
;; by Fu Jie Huang, Jan 2003

(libload "video4linux/v4l")
(libload "opencv/opencv")
(libload "opencv/aux_object")
(libload "libc/libc")


(de put_dots ( img dots count )
  (let ((numpoints (peek-int (==> count ptr_nth 0)))
	(ipoint  (new cvpoint 0 0))
	(fpoint  (new cvpoint2d32f 0 0)))
    (for (i 0 numpoints)
	 (==> fpoint copy (==> corners ptr_nth i))
	 (==> ipoint set_x (cvround (==> fpoint get_x)))
	 (==> ipoint set_y (cvround (==> fpoint get_y)))
	 (cvCircle  :img:p  :ipoint:p  2 (cv_rgb 0 0 255)  3) )
    ) )	    

(de find_dots ( img dots count )
  (let* ((width  (==> img get_width))
	 (height (==> img get_height))
	 (grayimg (new cvimage width height 8 1))
	 (eigimg  (new cvimage width height 32 1))
	 (tempimg (new cvimage width height 32 1)) )
    (cvSetImageCOI  :img:p 1)
    (cvCopy  :img:p  :grayimg:p  (gptr ())  )
    (cvGoodFeaturesToTrack  :grayimg:p   :eigimg:p   :tempimg:p   :corners:p  :count:p  0.1  10  (gptr ())  )
    ))
  

(setq width 640)
(setq height 480)

(new-window width height  "webcam")
(setq webcam (new v4ldevice "/dev/video0" "NTSC" 0 width height))

(setq captimg (ubyte-matrix height width 3))
(setq bufimg  (new cvimage width height 8 3))
(setq nextimg (new cvimage width height 8 3))

(setq bufgray (new cvimage width height 8 1))
(setq nextgray (new cvimage width height 8 1))

(setq disimg  (new cvimage width height 8 3))

(setq corners     (new cvsmobarray (memsize_cvpoint2d32f) 300))
(setq nextcorners (new cvsmobarray (memsize_cvpoint2d32f) 300))
(setq count   (new cvsmobarray 4 1))

(==> webcam grab-into-rgb captimg)

(idx8u_ipl  captimg  :bufimg:p)
(find_dots  bufimg  corners  count)
 
(idx8u_ipl  captimg  :disimg:p)
(put_dots   disimg  corners  count)
 
(cvimage-show disimg) 
;; (idx8u_ipl  captimg  :bufimg:p)

(setq status   (new cvsmobarray 1 300))
(setq winsize  (new cvsize 10 10))
(setq criteria (new cvtermcriteria (bitor @CV_TERMCRIT_ITER  @CV_TERMCRIT_EPS)  20  0.03 ))
 
(setq flag 0)
(setq  numpoints (peek-int (==> count ptr_nth 0)))
 (while t 
   (==> webcam grab-into-rgb captimg)
   (idx8u_ipl   captimg  :disimg:p)
 
   (when (= flag 0)
     (idx8u_ipl  captimg  :nextimg:p)
     (cvSetImageCOI  :nextimg:p 1)
     (cvcopy :nextimg:p   :nextgray:p (gptr ()) )
     (cvsetimagecoi  :bufimg:p  1)
     (cvcopy :bufimg:p    :bufgray:p (gptr ()) )
     (cvcalcOpticalFlowPyrLK  :bufgray:p  :nextgray:p  (gptr ()) (gptr ()) 
			      :corners:p  :nextcorners:p 
			      numpoints
			      :winsize:p  3
			      :status:p   (gptr ())
			      :criteria:p 0) 
     (setq flag 1) 
     (put_dots    disimg  nextcorners  count)    )
 
   (when (= flag 1)
     (idx8u_ipl  captimg  :bufimg:p)
     (cvSetImageCOI  :bufimg:p 1)
     (cvSetImageCOI  :nextimg:p 1)
     (cvcopy :nextimg:p   :nextgray:p (gptr ()) )
     (cvcopy :bufimg:p    :bufgray:p (gptr ()) )

     (cvcalcOpticalFlowPyrLK  :nextgray:p  :bufgray:p  (gptr ()) (gptr ()) 
			      :nextcorners:p  :corners:p 
			      numpoints
			      :winsize:p  3
			      :status:p   (gptr ())
			      :criteria:p 0) 
     (setq flag 0) 
     (put_dots    disimg  corners  count)    )    
 
   (cvimage-show disimg) )

    
    
    
  
