Alvaro Cassinelli / Mbed 2 deprecated laserUI

Laser Sensing Display for UI interfaces in the real world

Dependencies:   mbed

Fork of skinGames_forktest by Alvaro Cassinelli

LaserRenderer.h@47:199042980678, 2014-04-17 (annotated)

Committer:
mbedalvaro
Date:
Thu Apr 17 08:04:14 2014 +0000
Revision:
47:199042980678
Parent:
40:3ba2b0ea9f33 
publishing for sharing with Ken Iwasaki

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedalvaro 40:3ba2b0ea9f33 1 /*
mbedalvaro 40:3ba2b0ea9f33 2 ** Name: LaserRenderer.h
mbedalvaro 40:3ba2b0ea9f33 3
mbedalvaro 40:3ba2b0ea9f33 4 ** Description: THIS IS BASICALLY A "STATE MACHINE" with push/pop methods for RT, K, and some methods to modify/load these matrices; also, it provides methods to rendre objects and scenes, but
mbedalvaro 40:3ba2b0ea9f33 5 this may be done by using methods belonging to these objects instead (perhaps clearer).
mbedalvaro 40:3ba2b0ea9f33 6
mbedalvaro 40:3ba2b0ea9f33 7 ** Notes:
mbedalvaro 40:3ba2b0ea9f33 8 (1) I have separated the transformation (RT) from the projection (K) in the "render" methos. The reason is, we may want to FIRST get the list of all the points in 3d
mbedalvaro 40:3ba2b0ea9f33 9 coordinates, so we can apply a transformation in the future, without going through the process of re-building objects.
mbedalvaro 40:3ba2b0ea9f33 10
mbedalvaro 40:3ba2b0ea9f33 11 (2) the real "projection matrix" as defined in Hartley & Zisserman is P= K[R|t], a 3x4 matrix. K and R are 3x3 matrices, t is a 1x3 vector. P takes vector X (homogeneous)
mbedalvaro 40:3ba2b0ea9f33 12 to x (its projection, also in homogeneous coordinates).
mbedalvaro 40:3ba2b0ea9f33 13 It can be written as:
mbedalvaro 40:3ba2b0ea9f33 14 P = K[I|0][R|t] [R|t] [R|t]
mbedalvaro 40:3ba2b0ea9f33 15 [0 1], with K[I|0] a 3x4 matrix (K a 3x3 matrix) and [0 1] a 4x4 matrix: The matrix K and the matrix [0 1] are the matrices I call "intrinsics" (K) and "pose" (RT).
mbedalvaro 40:3ba2b0ea9f33 16 The advantages of doing this are clear: the product of the "pose" matrices will account for composition of rotations AND translation in homogeneous coordinates.
mbedalvaro 40:3ba2b0ea9f33 17 Note that we don't really care if after discarding the last component w (after multiplying by [I|0]), the 3d vector does not represent the real 3d coordinate, but just
mbedalvaro 40:3ba2b0ea9f33 18 some point in the ray. This is because K produces the projection in 2d ALSO in homogeneous coordinates, so that dividing by w=z (some z on the ray!) will give the real image point.
mbedalvaro 40:3ba2b0ea9f33 19 (but of course, we may WANT to obtain the real 3d coordiante perhaps; see matrixClass.h)
mbedalvaro 40:3ba2b0ea9f33 20 Also note that the only interest of having K as a 3x3 matrix instead of 2x3 is to be able to combine PROJECTION matrices: something we DON'T use!
mbedalvaro 40:3ba2b0ea9f33 21 More precisely, we have: RT=EXTRINSICSxMODELVIEW, with MODELVIEW being the pose of the object in CAMERA coordinates.
mbedalvaro 40:3ba2b0ea9f33 22 EXTRINSICS is loaded once, MODELVIEW changes (data sent by the computer).
mbedalvaro 40:3ba2b0ea9f33 23
mbedalvaro 40:3ba2b0ea9f33 24 (3) On the "scaleFactorProjector": it is necessary when we use a different projector resolution while "scanning" with the laser, say, we
mbedalvaro 40:3ba2b0ea9f33 25 have a laser scanner capable of 4095x4095 "pixels", but we form an image which is 600x600. If we then run a camera-projector calibration routine, the
mbedalvaro 40:3ba2b0ea9f33 26 final intrinsics matrix won't be right: the "pixel size" is wrong, by a factor 4096/600 - remember that the camera intrinsics contain the focal length
mbedalvaro 40:3ba2b0ea9f33 27 and the origin IN PIXELS units). This is the role of "scaleFactorProjector". Hopefully, it will be alwyas set to 1 (because we either correct the intrinsics
mbedalvaro 40:3ba2b0ea9f33 28 before loading, or because we calibrated the projector USING A METHOD THAT DOES NOT IMPLIES SUB-RESOLUTION "SCANNING")
mbedalvaro 40:3ba2b0ea9f33 29
mbedalvaro 40:3ba2b0ea9f33 30 */
mbedalvaro 40:3ba2b0ea9f33 31
mbedalvaro 40:3ba2b0ea9f33 32 #ifndef LASER_RENDERER_H
mbedalvaro 40:3ba2b0ea9f33 33 #define LASER_RENDERER_H
mbedalvaro 40:3ba2b0ea9f33 34
mbedalvaro 40:3ba2b0ea9f33 35 #include <vector>
mbedalvaro 40:3ba2b0ea9f33 36
mbedalvaro 40:3ba2b0ea9f33 37 #include "matrixClass.h"
mbedalvaro 40:3ba2b0ea9f33 38 #include "Scene.h"
mbedalvaro 40:3ba2b0ea9f33 39 #include "laserSensingDisplay.h"
mbedalvaro 40:3ba2b0ea9f33 40
mbedalvaro 40:3ba2b0ea9f33 41 //extern LaserRenderer lsr; // note: object is pre-instantiated in LaserRenderer.cpp
mbedalvaro 40:3ba2b0ea9f33 42
mbedalvaro 40:3ba2b0ea9f33 43 // ==============================================================================================================================
mbedalvaro 40:3ba2b0ea9f33 44
mbedalvaro 40:3ba2b0ea9f33 45 // Note: we can "pack" all this methods in an object, and make this object "global" (external, pre-instantiated), or just define all the functions global.
mbedalvaro 40:3ba2b0ea9f33 46 // I prefer the first approach, in case I need to extend the class, and also to avoid needed to define namespaces for functions with the same name. Anyway, we always can
mbedalvaro 40:3ba2b0ea9f33 47 // "wrap" the methods in global functions (like "begin/end"...) to make it looks more like normal openGL (see "WrapperFunctions.h").
mbedalvaro 40:3ba2b0ea9f33 48
mbedalvaro 40:3ba2b0ea9f33 49
mbedalvaro 40:3ba2b0ea9f33 50 class LaserRenderer
mbedalvaro 40:3ba2b0ea9f33 51 {
mbedalvaro 40:3ba2b0ea9f33 52 public:
mbedalvaro 40:3ba2b0ea9f33 53
mbedalvaro 40:3ba2b0ea9f33 54 LaserRenderer();
mbedalvaro 40:3ba2b0ea9f33 55 ~LaserRenderer();
mbedalvaro 40:3ba2b0ea9f33 56
mbedalvaro 40:3ba2b0ea9f33 57 void init(); // default init parameters for the pose, K
mbedalvaro 40:3ba2b0ea9f33 58 void close();// perhaps nothing needed (unless the displaying engine is part of this class, but I will use wrappers)
mbedalvaro 40:3ba2b0ea9f33 59
mbedalvaro 40:3ba2b0ea9f33 60 // Loading matrices from data sent by the computer or from a system file (or default global arrays):
mbedalvaro 40:3ba2b0ea9f33 61 // The results of pose estimation:
mbedalvaro 40:3ba2b0ea9f33 62 void loadPoseMatrix(float m[12]); // load RT[4][4]
mbedalvaro 40:3ba2b0ea9f33 63 // And the results form calibration:
mbedalvaro 40:3ba2b0ea9f33 64 void loadProjMatrix(float m[9], float scaleFactorProjector); // load K[3][3]
mbedalvaro 40:3ba2b0ea9f33 65 void loadExtrinsicsMatrix(float m[12]); //load EXTRINSICS[4][4]
mbedalvaro 40:3ba2b0ea9f33 66
mbedalvaro 40:3ba2b0ea9f33 67 void setIdentityPose(void); // Set RT=ID44
mbedalvaro 40:3ba2b0ea9f33 68 void setExtrinsicsPose(void); // directly sets RT=EXTRINSICS (i.e., the modelview is the identity...)
mbedalvaro 40:3ba2b0ea9f33 69 void setIdentityProjection(void); // Set K=ID33
mbedalvaro 40:3ba2b0ea9f33 70
mbedalvaro 40:3ba2b0ea9f33 71 // More advanced settings:
mbedalvaro 40:3ba2b0ea9f33 72 void setOrthoProjection(); // in the future, it can take parameters (clipping planes) as in glOrtho
mbedalvaro 40:3ba2b0ea9f33 73 //void setFrustumProjection(...); // to do
mbedalvaro 40:3ba2b0ea9f33 74
mbedalvaro 40:3ba2b0ea9f33 75 void setColor(unsigned char _c) {color=_c;}
mbedalvaro 40:3ba2b0ea9f33 76
mbedalvaro 40:3ba2b0ea9f33 77 // Euclidian transformations (operate on the right of RT):
mbedalvaro 40:3ba2b0ea9f33 78 void translate(float x, float y, float z);
mbedalvaro 40:3ba2b0ea9f33 79 void rotateX(float thetadeg);
mbedalvaro 40:3ba2b0ea9f33 80 void rotateY(float thetadeg);
mbedalvaro 40:3ba2b0ea9f33 81 void rotateZ(float thetadeg);
mbedalvaro 40:3ba2b0ea9f33 82 void flipX();
mbedalvaro 40:3ba2b0ea9f33 83 void flipY();
mbedalvaro 40:3ba2b0ea9f33 84 void flipZ();
mbedalvaro 40:3ba2b0ea9f33 85 void resize(float rx, float ry, float rz); // multiplies the current RT matrix by a diagonal resizing matrix
mbedalvaro 40:3ba2b0ea9f33 86
mbedalvaro 40:3ba2b0ea9f33 87 // Compose RT with an arbitrary transformation (multiplies the current pose matrix (RT) with m to give the new pose matrix RT):
mbedalvaro 40:3ba2b0ea9f33 88 void multPoseMatrix(const Mat44 m) ;
mbedalvaro 40:3ba2b0ea9f33 89 void multPoseMatrix(const float m[12]);
mbedalvaro 40:3ba2b0ea9f33 90
mbedalvaro 40:3ba2b0ea9f33 91 // Push/Pop methods: this is the main interest of this programming structure (I mean, the "state machine" for the rendering variables, useful to create
mbedalvaro 40:3ba2b0ea9f33 92 // complex objects with nested parts inheriting the current pose):
mbedalvaro 40:3ba2b0ea9f33 93 void pushPoseMatrix(void);
mbedalvaro 40:3ba2b0ea9f33 94 void popPoseMatrix(void);
mbedalvaro 40:3ba2b0ea9f33 95 void pushProjMatrix(void);
mbedalvaro 40:3ba2b0ea9f33 96 void popProjMatrix(void);
mbedalvaro 40:3ba2b0ea9f33 97 void pushColor(void);
mbedalvaro 40:3ba2b0ea9f33 98 void popColor(void);
mbedalvaro 40:3ba2b0ea9f33 99
mbedalvaro 40:3ba2b0ea9f33 100 // Point projection functions - made inline for efficiency:
mbedalvaro 40:3ba2b0ea9f33 101 V2 renderPointProj(const V3& v3);
mbedalvaro 40:3ba2b0ea9f33 102 V2 renderPointOrth(const V3& v3);
mbedalvaro 40:3ba2b0ea9f33 103
mbedalvaro 40:3ba2b0ea9f33 104 // Projection of whole objects and scenes:
mbedalvaro 40:3ba2b0ea9f33 105 void renderObject(BaseObject* ptObject);
mbedalvaro 40:3ba2b0ea9f33 106 void renderObject(BaseObject* ptObject, Mat44& moreRT); // applies supplemental transformation, but without modifying the original 3d points in vertexArray
mbedalvaro 40:3ba2b0ea9f33 107 void renderScene(Scene* ptr_scene);
mbedalvaro 40:3ba2b0ea9f33 108 void renderScene(Scene* ptr_scene, Mat44& moreRT); // applies supplemental transformation, but without modifying the original 3d points in vertexArray
mbedalvaro 40:3ba2b0ea9f33 109
mbedalvaro 40:3ba2b0ea9f33 110 //private:
mbedalvaro 40:3ba2b0ea9f33 111
mbedalvaro 40:3ba2b0ea9f33 112 Mat44 EXTRINSICS; // this is the camera-projector extrinsics, to be loaded (or set in hard) only ONCE in principle
mbedalvaro 40:3ba2b0ea9f33 113
mbedalvaro 40:3ba2b0ea9f33 114 Mat44 RT; // Current pose matrix (contains rotation AND translation) in projector coordinate frame (if we first set the pose to EXTRINSICS) or camera frame (if we
mbedalvaro 40:3ba2b0ea9f33 115 // first set the pose as the identity).
mbedalvaro 40:3ba2b0ea9f33 116 vector <Mat44> RT_Stack;
mbedalvaro 40:3ba2b0ea9f33 117
mbedalvaro 40:3ba2b0ea9f33 118 Mat33 K; // Current projection matrix (do we really need a stack here? probably not...)
mbedalvaro 40:3ba2b0ea9f33 119 float scaleFactorProjector;
mbedalvaro 40:3ba2b0ea9f33 120 vector <Mat33> K_Stack;
mbedalvaro 40:3ba2b0ea9f33 121 vector <float> scaleFactor_Stack;
mbedalvaro 40:3ba2b0ea9f33 122
mbedalvaro 40:3ba2b0ea9f33 123 unsigned char color; // current color
mbedalvaro 40:3ba2b0ea9f33 124 vector <unsigned char> color_Stack;
mbedalvaro 40:3ba2b0ea9f33 125
mbedalvaro 40:3ba2b0ea9f33 126 // ALSO, HERE, we can SET A VIEWPORT RANGE, DO SCALING OR SHEARS, etc...
mbedalvaro 40:3ba2b0ea9f33 127 // .... DO DO!!
mbedalvaro 40:3ba2b0ea9f33 128
mbedalvaro 40:3ba2b0ea9f33 129 // The Scene (a collection of objects):
mbedalvaro 40:3ba2b0ea9f33 130 // QUESTION: SHOULD THIS BE AN INSTANCE VARIABLE OF the LASER RENDERER OBJECT? the laser renderer object may be a GLOBAL object that can be used by ANY Scene,
mbedalvaro 40:3ba2b0ea9f33 131 // BaseObject, etc to get the proper modelview, projection matrix, colors, etc while building objects or rendering them...
mbedalvaro 40:3ba2b0ea9f33 132 // Scene myScene;
mbedalvaro 40:3ba2b0ea9f33 133 // (for the time being, I will make scene a GLOBAL. This seems clearer since we can use Scene/BaseObjects methods WITHTOUT the need to access data (pose, K) from the same lsr!!
mbedalvaro 40:3ba2b0ea9f33 134
mbedalvaro 40:3ba2b0ea9f33 135 // Finally, the displaying engine:
mbedalvaro 40:3ba2b0ea9f33 136 // laserSensingDisplay lsd;
mbedalvaro 40:3ba2b0ea9f33 137 // Again, I will make it GLOBAL. Wrapper functions will take care that the displaying engine is properly linked to the scene to display, as well as attach/detach the interrupt when
mbedalvaro 40:3ba2b0ea9f33 138 // modifying the scene...
mbedalvaro 40:3ba2b0ea9f33 139 };
mbedalvaro 40:3ba2b0ea9f33 140
mbedalvaro 40:3ba2b0ea9f33 141 // =================================================================================================================================================
mbedalvaro 40:3ba2b0ea9f33 142 // inline methods:
mbedalvaro 40:3ba2b0ea9f33 143
mbedalvaro 40:3ba2b0ea9f33 144 // Note: per-point rendering is actually just for tests... not efficient to call a function for each point in the object/scene
mbedalvaro 40:3ba2b0ea9f33 145 inline V2 LaserRenderer::renderPointProj(const V3& v3)
mbedalvaro 40:3ba2b0ea9f33 146 {
mbedalvaro 40:3ba2b0ea9f33 147 // V2 v2=((K*v3)*scaleFactorProjector);
mbedalvaro 40:3ba2b0ea9f33 148 V2 lp((K*v3)*scaleFactorProjector); // create new 2D coordinates point with projected coordinates. Note first the projection K*v3, leaving a v2 vector,
mbedalvaro 40:3ba2b0ea9f33 149 // then the simple rescaling by scaleFactorProjector (see note (3) above).
mbedalvaro 40:3ba2b0ea9f33 150 // Finally, check viewport limits (we could do non isotropic scaling too, but this is normally in K with different focal lengths... ):
mbedalvaro 40:3ba2b0ea9f33 151 // For the time being, we will just check that the values are not outside the DAC mirror range (0-4095). Note: it cannot be <0, because v2 is unsigned short (uint16_t).
mbedalvaro 40:3ba2b0ea9f33 152 if (lp.x > MAX_AD_MIRRORS) lp.x=MAX_AD_MIRRORS; // constrain( lp.x, minViewportX, maxViewportX);
mbedalvaro 40:3ba2b0ea9f33 153 else if (lp.x < MIN_AD_MIRRORS) lp.x=MIN_AD_MIRRORS; // constrain( lp.x, minViewportX, maxViewportX);
mbedalvaro 40:3ba2b0ea9f33 154 if (lp.y > MAX_AD_MIRRORS) lp.y=MAX_AD_MIRRORS;
mbedalvaro 40:3ba2b0ea9f33 155 else if (lp.y < MIN_AD_MIRRORS) lp.y=MIN_AD_MIRRORS;
mbedalvaro 40:3ba2b0ea9f33 156 return(lp);
mbedalvaro 40:3ba2b0ea9f33 157 }
mbedalvaro 40:3ba2b0ea9f33 158
mbedalvaro 40:3ba2b0ea9f33 159 // THIS IS IN FACT ORTHOGRAPHIC PROJECTION:
mbedalvaro 40:3ba2b0ea9f33 160 inline V2 LaserRenderer::renderPointOrth(const V3& v3)
mbedalvaro 40:3ba2b0ea9f33 161 {
mbedalvaro 40:3ba2b0ea9f33 162 V2 lp(v3.x, v3.y);
mbedalvaro 40:3ba2b0ea9f33 163 // Finally, check viewport limits (we could do non isotropic scaling too, but this is normally in K with different focal lengths... ):
mbedalvaro 40:3ba2b0ea9f33 164 if (lp.x > MAX_AD_MIRRORS) lp.x=MAX_AD_MIRRORS; // constrain( lp.x, minViewportX, maxViewportX);
mbedalvaro 40:3ba2b0ea9f33 165 else if (lp.x < MIN_AD_MIRRORS) lp.x=MIN_AD_MIRRORS; // constrain( lp.x, minViewportX, maxViewportX);
mbedalvaro 40:3ba2b0ea9f33 166 if (lp.y > MAX_AD_MIRRORS) lp.y=MAX_AD_MIRRORS;
mbedalvaro 40:3ba2b0ea9f33 167 else if (lp.y < MIN_AD_MIRRORS) lp.y=MIN_AD_MIRRORS;
mbedalvaro 40:3ba2b0ea9f33 168 return(lp);
mbedalvaro 40:3ba2b0ea9f33 169 }
mbedalvaro 40:3ba2b0ea9f33 170
mbedalvaro 40:3ba2b0ea9f33 171
mbedalvaro 40:3ba2b0ea9f33 172 #endif