;; by Fu Jie Huang, 2003
;; updated Oct 2006, reason is that 
;; in the new OpenCV (0.9.9), the semantic of the function
;; GoodFeaturesToTrack is slightly modified, it uses the 
;; content of "count" as maximal number of corners to detect
;; if the "count" is not a null pointer

(libload "opencv/opencv")
(libload "libc/libc")

(setq img0 (cvimage-read-rgb "lion.jpg"))
(setq img1 (cvimage-read-rgb "lion.jpg"))

(setq grayimg (new cvimage 640 480 8 1))    
(cvSetImageCOI  :img0:p  1)
(cvCopy  :img0:p  :grayimg:p  (gptr ())  )

(setq corners (new cvsmobarray 
		   (memsize_cvpoint2d32f) 300))

(setq count   (new cvsmobarray 4 1))
(poke-int (==> count ptr_nth 0) 1000)  ;; maximum

(setq eigimg  (new cvimage 640 480 32 1))
(setq tempimg (new cvimage 640 480 32 1))

(cvGoodFeaturesToTrack :grayimg:p :eigimg:p 
		       :tempimg:p   
		       :corners:p   ;; CvPoint2D32f *
		       :count:p     ;; int *
                       0.2          ;; quality level
		       10           ;; min distance 
		       (gptr ())    ;; mask
		       3            ;; block size
		       0            ;; use Harris?
		       0.04)        ;; k

(setq numpoints (peek-int (==> count ptr_nth 0)))
(setq ccolor (new CvScalar 255 0 0 0))

(for (i 0 (1- numpoints))
     (let* ((ipoint  (new cvpoint 0 0))
	    (fpoint  (new cvpoint2d32f 0 0)))
       (==> fpoint copy (==> corners ptr_nth i))
       (==> ipoint set_x (cvround (==> fpoint get_x)))
       (==> ipoint set_y (cvround (==> fpoint get_y)))

       (cvCircle :img1:p :ipoint:p 4 
		 (==> ccolor getp) 2 8 0) ) )
(cvimage-show img1)
(delete ccolor)
