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

matrixClass.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 #ifndef MATRIX_CLASS_H
mbedalvaro 40:3ba2b0ea9f33 2 #define MATRIX_CLASS_H
mbedalvaro 40:3ba2b0ea9f33 3
mbedalvaro 40:3ba2b0ea9f33 4 #include "myVectorClass.h"
mbedalvaro 40:3ba2b0ea9f33 5
mbedalvaro 40:3ba2b0ea9f33 6 // Note: the renderer needs very basic matrix operations (4x4 and 3x3), and I am not going to include any matrix library (we just need a product of matrices).
mbedalvaro 40:3ba2b0ea9f33 7 // So, I will define here the types used (as struct or class, so as to take advantage of the default copy constructor that is NOT shallow for the float arrays, and then can be
mbedalvaro 40:3ba2b0ea9f33 8 // stored in vector structures). Indeed, the thing stored in a Standard Library container must be assignable and copyable - arrays are neither. A solution is to wrap the array in a struct:
mbedalvaro 40:3ba2b0ea9f33 9 // MORE: The point to note is that the array members are not shallow copied, compiler automatically performs Deep Copy for array members!! (this is interesting, because
mbedalvaro 40:3ba2b0ea9f33 10 // it may seem that defining a member "float a[100]" is in fact defining a pointer, but this is not so! and array is NOT a pointer (float *a).
mbedalvaro 40:3ba2b0ea9f33 11
mbedalvaro 40:3ba2b0ea9f33 12 // More handy typedefs:
mbedalvaro 40:3ba2b0ea9f33 13 typedef vector2D<unsigned short> V2; // this is an IMAGE POINT (in projector image plane - i.e., the mirror azimuth and elevation)
mbedalvaro 40:3ba2b0ea9f33 14 // NOTE: unsigned short is two bytes, 0 to 65535 (16 bits, at least in the mbed). This is also uint16_t, perhaps better used for clarity.
mbedalvaro 40:3ba2b0ea9f33 15 typedef vector3D<float> V3;// this is a 3D point
mbedalvaro 40:3ba2b0ea9f33 16
mbedalvaro 40:3ba2b0ea9f33 17 // ------------ Pose matrices. For simplifiying calculations, I will add a [0 0 0 1] row to the 3x4 matrix of "extrinsics" [R | t] ----------------------------
mbedalvaro 40:3ba2b0ea9f33 18 class Mat44
mbedalvaro 40:3ba2b0ea9f33 19 {
mbedalvaro 40:3ba2b0ea9f33 20 public:
mbedalvaro 40:3ba2b0ea9f33 21 Mat44(); // (remember: if I declare a constructor, the compiler no longer provides an implicit default constructor)
mbedalvaro 40:3ba2b0ea9f33 22 Mat44(const float m[12]);
mbedalvaro 40:3ba2b0ea9f33 23 // ~Mat44(); use default
mbedalvaro 40:3ba2b0ea9f33 24
mbedalvaro 40:3ba2b0ea9f33 25 //copy contructor: do I need to specify it? no! because I am not using pointers to store data (beware: arrays are NOT pointers!), so copy is not shallow...
mbedalvaro 40:3ba2b0ea9f33 26 //Mat44 (const Mat44& M);
mbedalvaro 40:3ba2b0ea9f33 27
mbedalvaro 40:3ba2b0ea9f33 28 // Methods:
mbedalvaro 40:3ba2b0ea9f33 29 //copy:
mbedalvaro 40:3ba2b0ea9f33 30 void operator=( const Mat44& M );
mbedalvaro 40:3ba2b0ea9f33 31
mbedalvaro 40:3ba2b0ea9f33 32 // set:
mbedalvaro 40:3ba2b0ea9f33 33 void set(const float m[12]);
mbedalvaro 40:3ba2b0ea9f33 34 void setIdentity();
mbedalvaro 40:3ba2b0ea9f33 35
mbedalvaro 40:3ba2b0ea9f33 36 // matrix products:
mbedalvaro 40:3ba2b0ea9f33 37 Mat44 operator*( const Mat44& ) const;
mbedalvaro 40:3ba2b0ea9f33 38 Mat44 operator*(const float m[12]) const;
mbedalvaro 40:3ba2b0ea9f33 39 Mat44& operator*=( const Mat44& M);
mbedalvaro 40:3ba2b0ea9f33 40 Mat44& operator*=( const float m[12] );
mbedalvaro 40:3ba2b0ea9f33 41
mbedalvaro 40:3ba2b0ea9f33 42 // matrix vector operations:
mbedalvaro 40:3ba2b0ea9f33 43 V3 operator*(const V3&) const; //note: although this is not needed for the purposes of computing the final image projection, I will normalize coordinates by dividing by the w coordinate.
mbedalvaro 40:3ba2b0ea9f33 44
mbedalvaro 40:3ba2b0ea9f33 45 // data (private? not for the time being)
mbedalvaro 40:3ba2b0ea9f33 46 float at[4][4];
mbedalvaro 40:3ba2b0ea9f33 47 };
mbedalvaro 40:3ba2b0ea9f33 48
mbedalvaro 40:3ba2b0ea9f33 49
mbedalvaro 40:3ba2b0ea9f33 50 // ------------- Projection matrices - this is the "Intrinsics" matrix (or K, a 3x3 matrix) ---------------------------------------------------------------------
mbedalvaro 40:3ba2b0ea9f33 51 class Mat33
mbedalvaro 40:3ba2b0ea9f33 52 {
mbedalvaro 40:3ba2b0ea9f33 53 public:
mbedalvaro 40:3ba2b0ea9f33 54 Mat33();
mbedalvaro 40:3ba2b0ea9f33 55 Mat33(const float m[6]); // note: last row is always {0,0,1}, and is not loaded
mbedalvaro 40:3ba2b0ea9f33 56 // ~Mat33(); // use default
mbedalvaro 40:3ba2b0ea9f33 57
mbedalvaro 40:3ba2b0ea9f33 58 //copy:
mbedalvaro 40:3ba2b0ea9f33 59 void operator=( const Mat33& M );
mbedalvaro 40:3ba2b0ea9f33 60
mbedalvaro 40:3ba2b0ea9f33 61 // set:
mbedalvaro 40:3ba2b0ea9f33 62 void set(const float m[6]);
mbedalvaro 40:3ba2b0ea9f33 63 void setIdentity();
mbedalvaro 40:3ba2b0ea9f33 64
mbedalvaro 40:3ba2b0ea9f33 65 // matrix vector operation (att: this may seem weird: a 3d vector multiplied by a 3x3 matrix gives a 2d vector? In fact I am converting the 2d homogeneous coordinates to real image coord)
mbedalvaro 40:3ba2b0ea9f33 66 V2 operator*(const V3& _v3) const;
mbedalvaro 40:3ba2b0ea9f33 67
mbedalvaro 40:3ba2b0ea9f33 68 float at[3][3];
mbedalvaro 40:3ba2b0ea9f33 69 };
mbedalvaro 40:3ba2b0ea9f33 70
mbedalvaro 40:3ba2b0ea9f33 71 // =============================================================================================================================================================================
mbedalvaro 40:3ba2b0ea9f33 72
mbedalvaro 40:3ba2b0ea9f33 73 // NOTE: I have a lot of Flash memory on the Cortex M3 (512kb), but not much speed.. so, let's inline these operations to avoid functions calls on the following
mbedalvaro 40:3ba2b0ea9f33 74 // methods that are heavily called. SO, IN THAT CASE, the body of all inline function needs to be in the header so that the compiler can actually substitute it wherever required!!
mbedalvaro 40:3ba2b0ea9f33 75 // (this means, I won't be using a matrixClass.cpp file to contain the definitions. This is NOT going to be a problem for the compiler, as long as all the methods are INLINE (otherwise
mbedalvaro 40:3ba2b0ea9f33 76 // it will give "multiple definition" errors).
mbedalvaro 40:3ba2b0ea9f33 77
mbedalvaro 40:3ba2b0ea9f33 78 // =============================== Mat44 Class definitions =====================================
mbedalvaro 40:3ba2b0ea9f33 79
mbedalvaro 40:3ba2b0ea9f33 80 // Constructors:
mbedalvaro 40:3ba2b0ea9f33 81 inline Mat44::Mat44()
mbedalvaro 40:3ba2b0ea9f33 82 {
mbedalvaro 40:3ba2b0ea9f33 83 setIdentity();
mbedalvaro 40:3ba2b0ea9f33 84 }
mbedalvaro 40:3ba2b0ea9f33 85 inline Mat44::Mat44(const float m[12])
mbedalvaro 40:3ba2b0ea9f33 86 {
mbedalvaro 40:3ba2b0ea9f33 87 set(m);
mbedalvaro 40:3ba2b0ea9f33 88 }
mbedalvaro 40:3ba2b0ea9f33 89
mbedalvaro 40:3ba2b0ea9f33 90 // Destructor: use default
mbedalvaro 40:3ba2b0ea9f33 91 //Mat44::~Mat44() {}
mbedalvaro 40:3ba2b0ea9f33 92
mbedalvaro 40:3ba2b0ea9f33 93 inline void Mat44::operator=( const Mat44& M )
mbedalvaro 40:3ba2b0ea9f33 94 {
mbedalvaro 40:3ba2b0ea9f33 95 // do a deep copy of the data:
mbedalvaro 40:3ba2b0ea9f33 96 memcpy(&at[0][0], &(M.at[0][0]), sizeof(at)); // this is possible because array data is contiguously allocated
mbedalvaro 40:3ba2b0ea9f33 97 }
mbedalvaro 40:3ba2b0ea9f33 98
mbedalvaro 40:3ba2b0ea9f33 99 inline void Mat44::set(const float m[12]) // the matrix is loaded in ROW-COLUMN order (meaning, we fill rows by rows, or "row first")
mbedalvaro 40:3ba2b0ea9f33 100 {
mbedalvaro 40:3ba2b0ea9f33 101 for (int i=0; i<3; i++)
mbedalvaro 40:3ba2b0ea9f33 102 for (int j=0; j<4; j++) at[i][j]=m[4*i+j];
mbedalvaro 40:3ba2b0ea9f33 103 // Also, the last row is set to {0,0,0,1}:
mbedalvaro 40:3ba2b0ea9f33 104 at[3][0]=0; at[3][1]=0; at[3][2]=0; at[3][3]=1;
mbedalvaro 40:3ba2b0ea9f33 105 }
mbedalvaro 40:3ba2b0ea9f33 106 inline void Mat44::setIdentity()
mbedalvaro 40:3ba2b0ea9f33 107 {
mbedalvaro 40:3ba2b0ea9f33 108 static const float ID[12] = { 1, 0, 0, 0,
mbedalvaro 40:3ba2b0ea9f33 109 0, 1, 0, 0,
mbedalvaro 40:3ba2b0ea9f33 110 0, 0, 1, 0 };
mbedalvaro 40:3ba2b0ea9f33 111 set(ID); // we assume last row is {0,0,0,1}
mbedalvaro 40:3ba2b0ea9f33 112 }
mbedalvaro 40:3ba2b0ea9f33 113
mbedalvaro 40:3ba2b0ea9f33 114 inline Mat44 Mat44::operator*( const Mat44& A) const
mbedalvaro 40:3ba2b0ea9f33 115 {
mbedalvaro 40:3ba2b0ea9f33 116 Mat44 Res;
mbedalvaro 40:3ba2b0ea9f33 117 for (int i=0; i<4; i++) {
mbedalvaro 40:3ba2b0ea9f33 118 for (int j=0; j<4; j++) {
mbedalvaro 40:3ba2b0ea9f33 119 float sum=0;
mbedalvaro 40:3ba2b0ea9f33 120 for (int k=0; k<4; k++) sum+=at[i][k]*A.at[k][j];
mbedalvaro 40:3ba2b0ea9f33 121 Res.at[i][j]=sum;
mbedalvaro 40:3ba2b0ea9f33 122 }
mbedalvaro 40:3ba2b0ea9f33 123 }
mbedalvaro 40:3ba2b0ea9f33 124 return Res;
mbedalvaro 40:3ba2b0ea9f33 125 }
mbedalvaro 40:3ba2b0ea9f33 126
mbedalvaro 40:3ba2b0ea9f33 127 // use reduced matrix as float (sent by computer or defined by the user):
mbedalvaro 40:3ba2b0ea9f33 128 inline Mat44 Mat44::operator*(const float m[12]) const
mbedalvaro 40:3ba2b0ea9f33 129 {
mbedalvaro 40:3ba2b0ea9f33 130 Mat44 Res;
mbedalvaro 40:3ba2b0ea9f33 131 for (int i=0; i<4; i++) {
mbedalvaro 40:3ba2b0ea9f33 132 for (int j=0; j<4; j++) {
mbedalvaro 40:3ba2b0ea9f33 133 float sum=0;
mbedalvaro 40:3ba2b0ea9f33 134 for (int k=0; k<3; k++) sum+=at[i][k]*m[4*k+j]; // we assume the last raw of matrix "augmented" matrix m is {0, 0, 0, 1}
mbedalvaro 40:3ba2b0ea9f33 135 Res.at[i][j]=sum;
mbedalvaro 40:3ba2b0ea9f33 136 }
mbedalvaro 40:3ba2b0ea9f33 137 // we assume the last raw of matrix "augmented" matrix m is {0, 0, 0, 1}, so we need to add:
mbedalvaro 40:3ba2b0ea9f33 138 Res.at[i][3]+=at[i][3];
mbedalvaro 40:3ba2b0ea9f33 139 }
mbedalvaro 40:3ba2b0ea9f33 140 return Res;
mbedalvaro 40:3ba2b0ea9f33 141 }
mbedalvaro 40:3ba2b0ea9f33 142
mbedalvaro 40:3ba2b0ea9f33 143 inline Mat44& Mat44::operator*=(const Mat44& M )
mbedalvaro 40:3ba2b0ea9f33 144 {
mbedalvaro 40:3ba2b0ea9f33 145 float row[4];
mbedalvaro 40:3ba2b0ea9f33 146 for (int i=0; i<4; i++) {
mbedalvaro 40:3ba2b0ea9f33 147 for (int j=0; j<4; j++) {
mbedalvaro 40:3ba2b0ea9f33 148 float sum=0;
mbedalvaro 40:3ba2b0ea9f33 149 for (int k=0; k<4; k++) sum+=at[i][k]*M.at[k][j];
mbedalvaro 40:3ba2b0ea9f33 150 row[j]=sum;
mbedalvaro 40:3ba2b0ea9f33 151 }
mbedalvaro 40:3ba2b0ea9f33 152 // we don't use any more the row at[i][..], so we can update the result:
mbedalvaro 40:3ba2b0ea9f33 153 for (int k=0; k<4; k++) at[i][k]=row[k];
mbedalvaro 40:3ba2b0ea9f33 154 }
mbedalvaro 40:3ba2b0ea9f33 155 // return this object, so we can chain:
mbedalvaro 40:3ba2b0ea9f33 156 return *this;
mbedalvaro 40:3ba2b0ea9f33 157 }
mbedalvaro 40:3ba2b0ea9f33 158
mbedalvaro 40:3ba2b0ea9f33 159 // use reduced matrix as float (sent by computer or defined by the user):
mbedalvaro 40:3ba2b0ea9f33 160 inline Mat44& Mat44::operator*=( const float m[12] )
mbedalvaro 40:3ba2b0ea9f33 161 {
mbedalvaro 40:3ba2b0ea9f33 162 float row[4];
mbedalvaro 40:3ba2b0ea9f33 163 for (int i=0; i<4; i++) {
mbedalvaro 40:3ba2b0ea9f33 164 for (int j=0; j<4; j++) {
mbedalvaro 40:3ba2b0ea9f33 165 float sum=0;
mbedalvaro 40:3ba2b0ea9f33 166 for (int k=0; k<3; k++) sum+=at[i][k]*m[4*k+j]; // we assume the last raw of matrix "augmented" matrix m is {0, 0, 0, 1}
mbedalvaro 40:3ba2b0ea9f33 167 row[j]=sum;
mbedalvaro 40:3ba2b0ea9f33 168 }
mbedalvaro 40:3ba2b0ea9f33 169 // we don't use any more the row at[i][..], so we can update the result:
mbedalvaro 40:3ba2b0ea9f33 170 for (int k=0; k<3; k++) at[i][k]=row[k];
mbedalvaro 40:3ba2b0ea9f33 171 at[i][3]+=row[3];
mbedalvaro 40:3ba2b0ea9f33 172 }
mbedalvaro 40:3ba2b0ea9f33 173 // return this object, so we can chain:
mbedalvaro 40:3ba2b0ea9f33 174 return *this;
mbedalvaro 40:3ba2b0ea9f33 175 }
mbedalvaro 40:3ba2b0ea9f33 176
mbedalvaro 40:3ba2b0ea9f33 177 // Matrix vector operations:
mbedalvaro 40:3ba2b0ea9f33 178 // Note: this may seems weird: a 3d vector multiplied by a 4x4 matrix? in fact, I am assuming the 3d vector is not at infinity, so W=1, and then the result is normalized (not homogeneous)
mbedalvaro 40:3ba2b0ea9f33 179 inline V3 Mat44::operator*(const V3& _v3) const // note: second const means that this method does not changes any member variable
mbedalvaro 40:3ba2b0ea9f33 180 {
mbedalvaro 40:3ba2b0ea9f33 181 V3 Res3d; // note: this Res3d object is a local variable, but if we return it as an OBJECT (not pointer), then it will be first copied...
mbedalvaro 40:3ba2b0ea9f33 182 float w;
mbedalvaro 40:3ba2b0ea9f33 183
mbedalvaro 40:3ba2b0ea9f33 184 //note: although this is not needed for the purposes of computing the final image projection, I will normalize coordinates by dividing by the w coordinate:
mbedalvaro 40:3ba2b0ea9f33 185 w = at[3][0]*_v3.x + at[3][1]*_v3.y + at[3][2]*_v3.z+at[3][3]*1.0; // we assume 3d points are not at infinity with W=1 (when added using vertex() method)
mbedalvaro 40:3ba2b0ea9f33 186 // NOTE: it is NOT generally true that at[3][0], at[3][1] and at[3][2] are 0, and at[3][3]=1 after all the transformations (another way to say it,
mbedalvaro 40:3ba2b0ea9f33 187 // if we associate the product of matrices from the right: the homogeneous coordinates of the transformed point has W!=1)
mbedalvaro 40:3ba2b0ea9f33 188 Res3d.x = (at[0][0]*_v3.x + at[0][1]*_v3.y + at[0][2]*_v3.z+at[0][3]*1.0)/w;
mbedalvaro 40:3ba2b0ea9f33 189 Res3d.y = (at[1][0]*_v3.x + at[1][1]*_v3.y + at[1][2]*_v3.z+at[1][3]*1.0)/w;
mbedalvaro 40:3ba2b0ea9f33 190 Res3d.z = (at[2][0]*_v3.x + at[2][1]*_v3.y + at[2][2]*_v3.z+at[2][3]*1.0)/w;
mbedalvaro 40:3ba2b0ea9f33 191
mbedalvaro 40:3ba2b0ea9f33 192 return Res3d; // return a 3d vector (att: NOT homogeneous coordinates)
mbedalvaro 40:3ba2b0ea9f33 193 }
mbedalvaro 40:3ba2b0ea9f33 194
mbedalvaro 40:3ba2b0ea9f33 195 // =============================== Mat33 Class definitions =====================================
mbedalvaro 40:3ba2b0ea9f33 196 // Constructors:
mbedalvaro 40:3ba2b0ea9f33 197 inline Mat33::Mat33()
mbedalvaro 40:3ba2b0ea9f33 198 {
mbedalvaro 40:3ba2b0ea9f33 199 setIdentity();
mbedalvaro 40:3ba2b0ea9f33 200 }
mbedalvaro 40:3ba2b0ea9f33 201 inline Mat33::Mat33(const float m[6])
mbedalvaro 40:3ba2b0ea9f33 202 {
mbedalvaro 40:3ba2b0ea9f33 203 set(m);
mbedalvaro 40:3ba2b0ea9f33 204 }
mbedalvaro 40:3ba2b0ea9f33 205
mbedalvaro 40:3ba2b0ea9f33 206 inline void Mat33::operator=( const Mat33& M )
mbedalvaro 40:3ba2b0ea9f33 207 {
mbedalvaro 40:3ba2b0ea9f33 208 // do a deep copy of the data:
mbedalvaro 40:3ba2b0ea9f33 209 memcpy(&at[0][0], &M.at[0][0], sizeof(at)); // this is possible because array data is contiguously allocated in the "at" array (containing 9 floats)
mbedalvaro 40:3ba2b0ea9f33 210 }
mbedalvaro 40:3ba2b0ea9f33 211
mbedalvaro 40:3ba2b0ea9f33 212 inline void Mat33::set(const float m[6])
mbedalvaro 40:3ba2b0ea9f33 213 {
mbedalvaro 40:3ba2b0ea9f33 214 for (int i=0; i<2; i++)
mbedalvaro 40:3ba2b0ea9f33 215 for (int j=0; j<3; j++) at[i][j]=m[3*i+j]; // m is in "column-row" order
mbedalvaro 40:3ba2b0ea9f33 216 // Also, the last row is set to {0,0,1} by default (is this a good idea? the matrix may be set for orthographic projection...)
mbedalvaro 40:3ba2b0ea9f33 217 at[2][0]=0; at[2][1]=0; at[2][2]=1;
mbedalvaro 40:3ba2b0ea9f33 218 }
mbedalvaro 40:3ba2b0ea9f33 219
mbedalvaro 40:3ba2b0ea9f33 220 inline void Mat33::setIdentity()
mbedalvaro 40:3ba2b0ea9f33 221 {
mbedalvaro 40:3ba2b0ea9f33 222 static const float ID[6] = { 1, 0, 0,
mbedalvaro 40:3ba2b0ea9f33 223 0, 1, 0 };
mbedalvaro 40:3ba2b0ea9f33 224 set(ID); // last row set to {0,0,1}
mbedalvaro 40:3ba2b0ea9f33 225 }
mbedalvaro 40:3ba2b0ea9f33 226
mbedalvaro 40:3ba2b0ea9f33 227 // Matrix vector operations (note: the matrix is 3x3, the vector is 3x1, but the output is 2x1 because it's NOT in homogeneous coordinates.
mbedalvaro 40:3ba2b0ea9f33 228 inline V2 Mat33::operator*(const V3& _v3) const
mbedalvaro 40:3ba2b0ea9f33 229 {
mbedalvaro 40:3ba2b0ea9f33 230 V2 Res2d;
mbedalvaro 40:3ba2b0ea9f33 231 float w;
mbedalvaro 40:3ba2b0ea9f33 232
mbedalvaro 40:3ba2b0ea9f33 233 // w =( at[2][0]*_v3.x + at[2][1]*_v3.y + at[2][2]*_v3.z); // this is in fact unnecessary, since at[2][0] and at[2][1] are 0,
mbedalvaro 40:3ba2b0ea9f33 234 // Note that at[2][2] is usually 1 (when real perspective projection), but can be 0 in case of orthographic projection.
mbedalvaro 40:3ba2b0ea9f33 235 // HOWEVER, in case of orthographic projection, we will NOT call this function - otherwise we need a check to see if w==0...
mbedalvaro 40:3ba2b0ea9f33 236 w = _v3.z;
mbedalvaro 40:3ba2b0ea9f33 237 Res2d.x = (at[0][0]*_v3.x + at[0][1]*_v3.y + at[0][2]*_v3.z)/w;
mbedalvaro 40:3ba2b0ea9f33 238 Res2d.y = (at[1][0]*_v3.x + at[1][1]*_v3.y + at[1][2]*_v3.z)/w;
mbedalvaro 40:3ba2b0ea9f33 239
mbedalvaro 40:3ba2b0ea9f33 240 return Res2d; // return a 2d vector (att: NOT homogeneous coordinates)
mbedalvaro 40:3ba2b0ea9f33 241 // ALSO: do not forget to apply the rescale factor (this is necessary when we use a different projector resolution while "scanning" with the laser, say, we
mbedalvaro 40:3ba2b0ea9f33 242 // 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 243 // 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 244 // 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 245 // before loading, or because we calibrated the projector USING A METHOD THAT DOES NOT IMPLIES SUB-RESOLUTION "SCANNING")
mbedalvaro 40:3ba2b0ea9f33 246 }
mbedalvaro 40:3ba2b0ea9f33 247
mbedalvaro 40:3ba2b0ea9f33 248 #endif