Transform translate(const Vec3& delta) { Mat4x4 m, mInv; //m e mInv são a identidade, logo só os elementos //da última coluna precisam mudar m(0, 3) = delta.x; m(1, 3) = delta.y; m(2, 3) = delta.z; mInv(0, 3) = -delta.x; mInv(1, 3) = -delta.y; mInv(2, 3) = -delta.z; return Transform(m, mInv); }
Transform rotateY(float angle) { Mat4x4 m, mInv; //A constante abaixo é igual a PI/180.0 float angleRadians = angle*0.0174532925; float c = cos(angleRadians); float s = sin(angleRadians); float invC = cos(-angleRadians); float invS = sin(-angleRadians); m(0, 0) = c; m(0, 2) = s; m(2, 0) = -s; m(2, 2) = c; mInv(0, 0) = invC; mInv(0, 2) = invS; mInv(2, 0) = -invS; mInv(2, 2) = invC; return Transform(m, mInv); }
void CMirror::initWorldToMirrorTransformation() { // compute u,v,n coordinate system of the mirror CVec3df u=p1-p0; CVec3df v=p2-p0; n=cross(u,v); v=cross(n,u); n.normaliseDestructive(); u.normaliseDestructive(); v.normaliseDestructive(); // create matrix m which transforms a point from u,v,n coordinates to x,y,z coordinates CMatrix m(4,4); m(0,0)=(float) u[0]; m(1,0)=(float) u[1]; m(2,0)=(float) u[2]; m(3,0)=0.0; m(0,1)=(float) v[0]; m(1,1)=(float) v[1]; m(2,1)=(float) v[2]; m(3,1)=0.0; m(0,2)=(float) n[0]; m(1,2)=(float) n[1]; m(2,2)=(float) n[2]; m(3,2)=0.0; m(0,3)=(float) p0[0]; m(1,3)=(float) p0[1]; m(2,3)=(float) p0[2]; m(3,3)=1.0; // Initialise the OpenGL transformation matrices using the // previously computed transformation // NOTE: As OpenGL matrices are defined columnwise, we have to // use the TRANSPOSE of the matrix. int i,j; for(i=0;i<4;i++) for(j=0;j<4;j++) mirrorToWorldTransformation[i][j]=(GLfloat) m(j,i); // store transpose CMatrix mInv=m.inverse(); for(i=0;i<4;i++) for(j=0;j<4;j++) worldToMirrorTransformation[i][j]=(GLfloat) mInv(j,i); // store transpose }
// note: assumes this matrix only // does translation and rotation; // m.Mul(m.Invert()) is identity CMatrix44f CMatrix44f::Invert() const { CMatrix44f mInv(*this); // transpose the rotation mInv[1] = m[4]; mInv[4] = m[1]; mInv[2] = m[8]; mInv[8] = m[2]; mInv[6] = m[9]; mInv[9] = m[6]; // get the inverse translation float3 t(-m[12], -m[13], -m[14]); // do the actual inversion mInv[12] = t.x * mInv[0] + t.y * mInv[4] + t.z * mInv[ 8]; mInv[13] = t.x * mInv[1] + t.y * mInv[5] + t.z * mInv[ 9]; mInv[14] = t.x * mInv[2] + t.y * mInv[6] + t.z * mInv[10]; return mInv; }
CMatrix44f CMatrix44f::InvertAffine() const { CMatrix44f mInv(*this); mInv.InvertAffineInPlace(); return mInv; }
void EKF_Correction(ekf_data xpred, ekf_data *xcorr, float *obs ){ float expMeas[NUM_SENS]; // Expected measurements int idseg[NUM_SENS]; // Segment id for expected measurements int idJac[NUM_SENS]; // To store the indexes required to build the Jacobian // float px_s, py_s, th_s; // Sensor odo odo s_odo[NUM_SENS]; float cth, sth; int i; int h_line; // Number of lines for the Jacobian float **mJ; float **dMeas; // z-h cth=cos(xpred.th); sth=sin(xpred.th); // Compute the expected measurements for (i=0; i<NUM_SENS; i++) { // pxs= px + cos(th)*px_sb -sin(th)*py_sb s_odo[i].x= xpred.x+cth*sa[i][SX]-sth*sa[i][SY]; // pys= py + sin(th)*px_sb +cos(th)*py_sb s_odo[i].y= xpred.y+sth*sa[i][SX]+cth*sa[i][SY]; // pth_s= th + th_sb s_odo[i].th= xpred.th+sa[i][STH]; ExpSensReading(s_odo[i], expMeas+i, idseg+i); #ifdef EKF_DEBUG printf("expM: %f\tidseg: %d\n",expMeas[i],idseg[i]); printf("s_b -- x: %f\ty: %f\t th: %f\n",sa[i][SX],sa[i][SY],sa[i][STH]); printf("s_odo -- x: %f\ty: %f\t th: %f\n",s_odo[i].x,s_odo[i].y,s_odo[i].th); #endif } // Let us find out the good observations along with the good expected measurements h_line=0; for (i=0; i<NUM_SENS; i++) { idJac[i]=-1; if ( obs[i] < MAX_DIST && expMeas[i]<MAX_DIST) { idJac[h_line]=i; // Id of the "used" sensors h_line++; } } //#ifdef EKF_DEBUG printf("\nh_line: %d\n", h_line); for (i=0; i<NUM_SENS; i++) { printf("%d [%d]\n",idJac[i],i); } //#endif // Main data Structure for the Jacobian // mJ = (float **) malloc(sizeof(float *)* h_line); mBuild(&mJ, h_line, X_SIZE); // Compute the Jacobian for (i=0; i<h_line; i++) { float num, den, d_num, d_den, s_num, s_den; float cths, sths; // *(mJ+i)=(float *) malloc(sizeof(float)*X_SIZE); // Compute the Jacobian // Let us consider the prediction robot odo x=(px, py, th) // Let us consider the sensor base odometry (px_sb, py_sb, th_sb) // Let us consider the sensor odometry (px_s, py_s, th_s) // | a*px_s + b*py_s +c | |num| // h(x,M) = ------------------------ = ---- // | a*cos(th_s) + b*sin(th_s) |den| // with: // pxs= px + cos(th)*px_sb -sin(th)*py_sb // pys= py + sin(th)*px_sb +cos(th)*py_sb // pth_s= th + th_sb // cths=cos(s_odo[idJac[i]].th); sths=sin(s_odo[idJac[i]].th); num = map[idseg[idJac[i]]][A]*s_odo[idJac[i]].x + map[idseg[idJac[i]]][B]*s_odo[idJac[i]].y + map[idseg[idJac[i]]][C]; s_num = sign(num); den = map[idseg[idJac[i]]][A]*cths+ map[idseg[idJac[i]]][B]*sths; s_den = sign(den); // // d num d |num| d num // ------ = a --> ------- = sign(num)* ------ = sign(num) * a // d px d px d px // // d num d |num| d num // ------ = b --> ------- = sign(num)* ------ = sign(num) * b // d py d py d py // // d num // d_num = ------ = a*(-px_sb*sin(th)-py_sb*cos(th))+b*(px_sb*cos(th)-py_sb*sin(th)) // d th // d num // d_den = ------ = -a*sin(th_s)+b*cos(th_s) // d th // // d_num = map[idseg[idJac[i]]][A]*(-sa[idJac[i]][SX]*sth-sa[idJac[i]][SY]*cth)+ map[idseg[idJac[i]]][B]* (sa[idJac[i]][SX]*cth -sa[idJac[i]][SY]*sth ); d_den = -map[idseg[idJac[i]]][A]*sths + map[idseg[idJac[i]]][B]*cths; // Let us build the Jacobian // // d h 1 d |num| 1 // ---- = ----- * ------- = ----- * sign(num) * a // d px |den| d px |den| // mJ[i][0]=s_num*map[idseg[idJac[i]]][A]/fabs(den); // d h 1 d |num| 1 // ---- = ----- * ------- = ----- * sign(num) * b // d py |den| d py |den| // mJ[i][1]=s_num*map[idseg[idJac[i]]][B]/fabs(den);; // d h sign(num)*d_num*den - sign(den)*d_den*num // ---- = ------------------------------------------ // d tx den^2 // mJ[i][2]=(s_num*d_num*den - s_den*d_den*num)/(den*den); } //#ifdef EKF_DEBUG printf("\n"); for (i=0; i<NUM_SENS; i++) printf("Sensor: %d\tReading: %f\tWall: %d\n",i+1,expMeas[i],(idseg[i]>-1)?idseg[i]+1:idseg[i]); printf("Xpre --- x: %f\ty: %f\t th: %f\n",xpred.x,xpred.y,xpred.th); //#endif for (i=0; i<h_line; i++) printf("Jx: %f\tJy: %f\tJth: %f\n",mJ[i][0],mJ[i][1],mJ[i][2]); float **mP1; float **mP2; float **mP3; float **mK; // Let us build the matrix // P (n x n) J (q x n) // P1=mPpred*J^T (n x n) x (n x q) = (n x q) mBuild(&mP1,X_SIZE,h_line); mMultTr2(mP1, X_SIZE, h_line, mPpred, X_SIZE, X_SIZE, mJ, h_line, X_SIZE); // printf("\nP1=Ppred*J^T\n"); // mPrint(mP1,X_SIZE,h_line); // P2= J*P1 (q x n) x (n x q) = (q x q) mBuild(&mP2,h_line,h_line); mMult(mP2,h_line, h_line, mJ, h_line, X_SIZE, mP1, X_SIZE, h_line); // printf("\nP2=J*P1\n"); // mPrint(mP2,h_line,h_line); // P2= P2+R (q x q) + (q x q) = (q x q) for (i=0; i<h_line; i++) mP2[i][i]+=vectR[idJac[i]]; // printf("\nP2=P2+R\n"); // mPrint(mP2,h_line,h_line); // P3= inv(P2) (q x q) mBuild(&mP3,h_line,h_line); mInv(mP3,h_line,h_line,mP2,h_line,h_line); // printf("\nP3=inv(P2)\n"); // mPrint(mP3,h_line,h_line); // K= P1*P3 (n x q) x (q x q) = (n x q) mBuild(&mK,X_SIZE,h_line); mMult(mK,X_SIZE,h_line,mP1, X_SIZE, h_line, mP3, h_line, h_line); printf("\nK=P1*P3\n"); mPrint(mK,X_SIZE,h_line); // To build the innovation vector mBuild(&dMeas, h_line,1); for (i=0; i<h_line; i++) { dMeas[i][0]=obs[idJac[i]]-expMeas[idJac[i]]; } // mPrint(dMeas,h_line,1); float **mP4; float **mP5; float **mInn; // Pk = (I-KJ)P // K*J // (n x q) x (q x n) = (n x n) mBuild(&mP4,X_SIZE,X_SIZE); mMult(mP4,X_SIZE,X_SIZE,mK,X_SIZE,h_line,mJ,h_line,X_SIZE); // printf("\nmJ\n"); // mPrint(mJ,h_line,X_SIZE); // printf("\nmP4\n"); // mPrint(mP4,X_SIZE,X_SIZE); // (I-K*J) mEye(&mP5,X_SIZE); mSub2(mP5,X_SIZE,X_SIZE,mP4,X_SIZE,X_SIZE); // (I-K*J)*P mMult(mPcorr, X_SIZE, X_SIZE, mP5, X_SIZE, X_SIZE, mPpred, X_SIZE, X_SIZE); printf("\nmPcorr\n"); mPrint(mPcorr,X_SIZE,X_SIZE); // K*vec // (n x h_line) x (h_line x 1) mBuild(&mInn, X_SIZE, 1); mMult(mInn, X_SIZE, 1, mK, X_SIZE, h_line, dMeas, h_line, 1); printf("\ndMeas\n"); mPrint(dMeas,h_line,1); printf("\nmInn\n"); mPrint(mInn,X_SIZE,1); // Xpost xcorr->x = xpred.x + mInn[0][0]; xcorr->y = xpred.y + mInn[1][0]; xcorr->th = xpred.th + mInn[2][0]; // Free Memory mFree(mJ,h_line); mFree(mP1,X_SIZE); mFree(mP2,h_line); mFree(mP3,h_line); mFree(mP4,X_SIZE); mFree(mP5,X_SIZE); mFree(mK,X_SIZE); mFree(mInn,X_SIZE); mFree(dMeas,h_line); }
int main(int argc, char *argv[]) { int i,j; float **m1; float **m2; float **m3; float **m4; float **m5; float *diag4; float **m1T; float **M1; float M2[5][5]; int row1, col1; // n x m int row1T,col1T; // m x n int row2, col2; // n x n int row3, col3; // n x n int row4, col4; // n x n int row5, col5; // n x n if (argc<2) { printf("Usage: %s randSeed\n",argv[0]); return -1; } // Test of functions that allocate the memory required to store the resulting matrix row1=ROW; col1=COL; mRand(&m1,row1,col1,10,atoi(argv[1])); mPrint(m1,row1,col1); // Transposition mTranspB(&m1T,&row1T,&col1T,m1,row1,col1); mPrint(m1T,row1T,col1T); // Multiplication mMultB(&m2,&row2,&col2,m1,row1,col1,m1T,row1T,col1T); mPrint(m2,row2,col2); // Inversion mInvB(&m3,&row3,&col3,m2,row2,col2); mPrint(m3,row3,col3); // Diagonal matrix row4=ROW; col4=ROW; diag4= (float *) malloc(sizeof(float)*row4); for (i=0; i<row4; i++) diag4[i]=i+1; mDiag(&m4,row4,diag4); mPrint(m4,row4,col4); mFree(m1,row1); mFree(m1T,row1T); mFree(m2,row2); mFree(m3,row3); mFree(m4,row4); // Test of functions that assume the memory required to store the // resulting matrix to be already allocated // row1=row2=row3=row4=row1T=0; // col1=col2=col3=col4=col1T=0; // Random Creation mRand(&m1,row1,col1,10,atoi(argv[1])); mPrint(m1,row1,col1); mBuild(&m1T,row1T,col1T); mBuild(&m2,row2,col2); mBuild(&m3,row3,col3); mBuild(&m4,row4,col4); // Transposition mTransp(m1T,row1T,col1T,m1,row1,col1); mPrint(m1T,row1T,col1T); // Multiplication mMult(m2,row2,col2,m1,row1,col1,m1T,row1T,col1T); mPrint(m2,row2,col2); // Inversion mInv(m3,row3,col3,m2,row2,col2); mPrint(m3,row3,col3); // Diagonal matrix row4=ROW; col4=ROW; diag4= (float *) malloc(sizeof(float)*row4); for (i=0; i<row4; i++) diag4[i]=i+1; // mDiag(&m4,row4,diag4); // mPrint(m4,row4,col4); // Mult mPrint(m1T,row1T,col1T); mFree(m2,row2); mRand(&m2,row2,col2,10,atoi(argv[1])*5); mPrint(m2,row2,col2); mFree(m4,row4); row4=row2; col4=row1T; mBuild(&m4,row4,col4); mMultTr2(m4,row4,col4,m2,row2,col2,m1T,row1T,col1T); mPrint(m4,row4,col4); // Copy row5=row4; col5=col4; printf("\ncopy\n"); mBuild(&m5,row5,col5); mPrint(m5,row5,col5); mCopy(m5,row5,col5,m4,row4,col4); mPrint(m4,row4,col4); mPrint(m5,row5,col5); mFree(m1,row1); mFree(m1T,row1T); mFree(m2,row2); mFree(m3,row3); mFree(m4,row4); // To check copy to and from float[r][c] // So far only squared matrix are copied! printf("\nLast\n"); mRand(&M1,5,5,10,atoi(argv[1])*5); mPrint(M1,5,5); mCopyM2A(5,M2,M1); for(i=0; i<5; i++) for(j=0; j<5; j++) M2[i][j]*=100; mCopyA2M(M1,5,M2); mPrint(M1,5,5); return 0; }