diff --git a/irteus/demo/sample-camera-model.l b/irteus/demo/sample-camera-model.l new file mode 100644 index 000000000..859056def --- /dev/null +++ b/irteus/demo/sample-camera-model.l @@ -0,0 +1,36 @@ +;; create camera and camera viewer +(setq *camera-model* + (make-camera-from-param :pwidth 640 :pheight 360 + :fx 400 :fy 400 + :cx 319.5 :cy 179.5 :name "camtest" + :create-viewer t)) +;; move camera +(send *camera-model* :translate #f(0 100 0) :world) +(send *camera-model* :rotate 0.25 :x :world) + +;; make objects +(setq *obj1* (make-cube 100 100 100)) +(send *obj1* :translate #f(-50 0 235)) +(send *obj1* :set-color #f(0 0 1)) +(setq *obj2* (make-cube 100 100 100)) +(send *obj2* :translate #f(50 0 265)) +(send *obj2* :set-color #f(1 0 0)) +(setq *obj3* (make-cube 100 100 100)) +(send *obj3* :translate #f(0 100 250)) +(send *obj3* :set-color #f(0 1 0)) +(objects (list *obj1* *obj2* *obj3* *camera-model*)) + + +;; draw objects on camera viewer +(send *camera-model* :draw-objects (list *obj1* *obj2* *obj3*)) + +;; get image and point cloud +(let ((ret (send *camera-model* :get-image :with-points t :with-colors t))) + (setq *image* (car ret)) + (setq *points* (cdr ret)) + ) + +;; transform origin of point cloud +(send *points* :transform (send *camera-model* :worldcoords)) + +(objects (list *points* *camera-model*)) diff --git a/irteus/irtgl.l b/irteus/irtgl.l index ad4e706cb..027ba3805 100644 --- a/irteus/irtgl.l +++ b/irteus/irtgl.l @@ -183,7 +183,7 @@ "Get current view to a image object. It returns color-image24 object." (let () (send self :makecurrent) - (glReadBuffer GL_BACK) + (glReadBuffer GL_FRONT) (glPixelStorei GL_PACK_ALIGNMENT 1) (glReadPixels x y width height GL_RGB GL_UNSIGNED_BYTE imgbuf) #-:x86_64 diff --git a/irteus/irtsensor.l b/irteus/irtsensor.l index 0a878dff0..e7cbda45d 100644 --- a/irteus/irtsensor.l +++ b/irteus/irtsensor.l @@ -78,7 +78,7 @@ (:projection :newprojection :view :viewpoint :view-direction :viewdistance :yon :hither)) - pwidth pheight)) + img-viewer pwidth pheight)) (defmethod camera-model (:init (b &rest args @@ -109,6 +109,7 @@ (:width () "Returns width of the camera in pixel." pwidth) (:height () "Returns height of the camera in pixel." pheight) (:viewing (&rest args) (forward-message-to vwing args)) + (:image-viewer (&rest args) (forward-message-to img-viewer args)) (:fovy () "Returns field of view in degree" (let ((proj (send vwing :projection))) (* 2 (atan2 (/ pwidth 2.0) (aref proj 0 0))))) @@ -190,7 +191,8 @@ (send vwer :viewsurface :color pcolor) (gl::draw-glbody vwer self) (if flush (send vwer :viewsurface :flush)))) - (:draw-objects + (:draw-objects (objs) (send self :draw-objects-raw img-viewer objs)) + (:draw-objects-raw (vwr objs) (let* (pcurrent pcolor (draw-things (x::draw-things objs)) viewpoint viewtarget @@ -199,6 +201,9 @@ (f (aref proj 0 0))) (send vwr :viewsurface :makecurrent) ;;(resetperspective (send vwr :viewing) (send vwr :viewsurface)) + (if (> pwidth pheight) + (gl::glviewport 0 (- (/ (- pwidth pheight) 2)) pwidth pwidth) + (gl::glviewport (- (/ (- pheight pwidth) 2)) 0 pheight pheight)) (gl::glMatrixMode gl::GL_PROJECTION) (gl::glLoadIdentity) ;; the following should get aspect ration from viewport @@ -209,12 +214,13 @@ (setq viewpoint (v+ (send self :worldpos) ;; for right camera (send self :viewing :rotate-vector - (scale 1000.0 (float-vector (/ (- (- (/ pwidth 2.0) 1) cx) f) - (/ (- (- (/ pheight 2.0) 1) cy) f) + (scale 1000.0 (float-vector (/ (- (/ (1- pwidth) 2.0) cx) f) + (/ (- (/ (1- pheight) 2.0) cy) f) 0))))) ;; glview define view-directoin to oppsite direction (setq viewtarget (v- viewpoint (send self :viewing :view-direction) )) + (pprint (list viewpoint viewtarget (v- (send self :viewing :view-up)))) (gl::gluLookAtfv (concatenate vector viewpoint viewtarget (v- (send self :viewing :view-up)))) (gl::glMatrixMode gl::GL_MODELVIEW) @@ -236,7 +242,27 @@ ) (send vwr :viewsurface :flush) )) - (:get-image (vwr &key (points) (colors)) + (:get-image + (&key (with-points) (with-colors)) + (let (points colors img pc) + (if with-points + (setq points (make-matrix (* pwidth pheight) 3))) + (if with-colors + (setq colors (make-matrix (* pwidth pheight) 3))) + (setq img (send self :get-image-raw img-viewer :points points :colors colors)) + (cond + ((and with-points with-colors) + (setq pc (instance pointcloud :init + :height pheight :width pwidth + :points points :colors colors))) + (with-points + (setq pc (instance pointcloud :init + :height pheight :width pwidth + :points points)))) + (if with-points + (cons img pc) + img))) + (:get-image-raw (vwr &key (points) (colors)) (let* ((sf (send vwr :viewsurface)) (width (send sf :width)) (height (send sf :height)) @@ -256,7 +282,7 @@ (when points (unless (and (= width pwidth) (= height pheight)) (warn ";; width: %d /= %d or height: %d /= %d~%" width pwidth height pheight) - (return-from :get-image)) + (return-from :get-image-raw)) (setq fv (make-array num :element-type :float)) (setq mat-ent (array-entity points)) (fill mat-ent 0.0) @@ -271,9 +297,10 @@ (dotimes (x width) (if (< (elt fv vptr) 1.0) (let ((zpos (/ (* fp np) (- (* (elt fv vptr) (- fp np)) fp)))) + (setq mptr (* 3 (+ (* (- height y 1) width) x))) (setf (elt pos 0) (* (- cx x) (/ zpos focus))) - (setf (elt pos 1) (* (- y cy) (/ zpos focus))) - (setf (elt pos 2) (- zpos)) + (setf (elt pos 1) (* (- cy (- height y 1)) (/ zpos focus))) + (setf (elt pos 2) (- zpos)) ;; ok (sys::vector-replace mat-ent pos mptr) (when colors (setf (elt col 0) @@ -283,7 +310,6 @@ (setf (elt col 2) (/ (sys::peek img-ent (+ mptr 2) :byte) 255.0)) (sys::vector-replace col-ent col mptr)))) - (incf mptr 3) (incf vptr 1) ))) img @@ -291,7 +317,8 @@ ) ;; utility functions -(defun make-camera-from-param (&key pwidth pheight fx fy cx cy (tx 0) (ty 0) parent-coords name) +(defun make-camera-from-param (&key pwidth pheight fx fy cx cy (tx 0) (ty 0) parent-coords name + create-viewer) "Create camera object from given parameters." (let* ((b (body+ (make-cube 40 30 30) (send (make-cylinder 2 30) :rotate pi/2 :x) @@ -310,9 +337,21 @@ (send c :translate (float-vector (- tx) (- ty) 0)) (send (c . vwing) :translate (float-vector tx ty 0)) (if parent-coords (send parent-coords :assoc c)) + (when create-viewer + (unless (boundp '*irtviewer*) (make-irtviewer)) + (let ((cv + (view + :x pwidth :y pheight + :viewing (send c :viewing) + :viewsurface + (instance gl::glviewsurface :create + :glcon ((send *irtviewer* :viewer :viewsurface) . gl::glcon) + :title (format nil "~A_view" name) :width pwidth :height pheight) + :title (format nil "~A_view" name)))) + (setq (c . img-viewer) cv) + )) c)) - (in-package "GEOMETRY") (provide :irtsensor "$Id: $")