Merge pull request #268 from YoheiKakiuchi/fix_camramodel

Fix camera model on irtsensor

Author: YoheiKakiuchi

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*))

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

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,15 +310,15 @@
                    (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
      ))
   )
 
 ;; 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: $")