MbICP.c
Go to the documentation of this file.
00001 /*************************************************************************************/
00002 /*                                                                              */
00003 /*  File:          MbICP.h                                                      */
00004 /*  Authors:       Luis Montesano and Javier Minguez                            */
00005 /*  Modified:      1/3/2006                                                     */
00006 /*                                                                              */
00007 /*  This library implements the:                                                */
00008 /*                                                                              */
00009 /*      J. Minguez, F. Lamiraux and L. Montesano                                */
00010 /*      Metric-Based Iterative Closest Point,                                   */
00011 /*  Scan Matching for Mobile Robot Displacement Estimation                      */
00012 /*      IEEE Transactions on Roboticics (2006)                                  */
00013 /*                                                                                   */
00014 /*************************************************************************************/
00015 
00016 
00017 #include "MbICP.h"
00018 #include "MbICP2.h"
00019 #include "calcul.h"
00020 #include "sp_matrix.h"
00021 #include <stdio.h>
00022 #include <math.h>
00023 #include "percolate.h"
00024 
00025 #ifndef M_PI
00026         #define M_PI 3.14159265358979323846
00027 #endif
00028 
00029 // Initial error to compute error ratio
00030 #define BIG_INITIAL_ERROR 1000000.0F
00031 
00032 // Debugging flag. Print sm info in the screen.
00033 // #define INTMATSM_DEB
00034 
00035 
00036 // ---------------------------------------------------------------
00037 // ---------------------------------------------------------------
00038 // Types definition
00039 // ---------------------------------------------------------------
00040 // ---------------------------------------------------------------
00041 
00042 
00043 
00044 // ---------------------------------------------------------------
00045 // ---------------------------------------------------------------
00046 // Variables definition
00047 // ---------------------------------------------------------------
00048 // ---------------------------------------------------------------
00049 
00050 float MAXLASERRANGE;
00051 
00052 
00053 // ************************
00054 // Extern variables
00055 
00056 // structure to initialize the SM parameters
00057 TSMparams params;
00058 
00059 // Original points to be aligned
00060 Tscan ptosRef;
00061 Tscan ptosNew;
00062 
00063 // Structure of the associations before filtering
00064 TAsoc cp_associations[MAXLASERPOINTS];
00065 int cntAssociationsT;
00066 
00067 // Filtered Associations
00068 TAsoc cp_associationsTemp[MAXLASERPOINTS];
00069 int cntAssociationsTemp;
00070 
00071 // Those points removed by the projection filter
00072 Tscan ptosNoView;
00073 
00074 // Current motion estimation
00075 Tsc motion2;
00076 
00077 
00078 // ************************
00079 // Some precomputations for each scan to speed up
00080 static float refdqx[MAXLASERPOINTS];
00081 static float refdqx2[MAXLASERPOINTS];
00082 static float refdqy[MAXLASERPOINTS];
00083 static float refdqy2[MAXLASERPOINTS];
00084 static float distref[MAXLASERPOINTS];
00085 static float refdqxdqy[MAXLASERPOINTS];
00086 
00087 
00088 // value of errors
00089 static float error_k1;
00090 static int numConverged;
00091 
00092 
00093 // ---------------------------------------------------------------
00094 // ---------------------------------------------------------------
00095 // Protos of the functions
00096 // ---------------------------------------------------------------
00097 // ---------------------------------------------------------------
00098 
00099 // Function for compatibility with the scans
00100 static void preProcessingLib(Tpfp *laserK, Tpfp *laserK1,
00101                                           Tsc *initialMotion);
00102 
00103 // Function that does the association step of the MbICP
00104 static int EStep();
00105 
00106 // Function that does the minimization step of the MbICP
00107 static int MStep(Tsc *solucion);
00108 
00109 // Function to do the least-squares but optimized for the metric
00110 static int computeMatrixLMSOpt(TAsoc *cp_ass, int cnt, Tsc *estimacion);
00111 
00112 // ---------------------------------------------------------------
00113 // ---------------------------------------------------------------
00114 // External functions
00115 // ---------------------------------------------------------------
00116 // ---------------------------------------------------------------
00117 
00118 // ************************
00119 // Function that initializes the SM parameters
00120 // ************************
00121 
00122 void Init_MbICP_ScanMatching(float max_laser_range,float Bw, float Br,
00123                                           float L, int laserStep,
00124                                           float MaxDistInter,
00125                                           float filter,
00126                                           int ProjectionFilter,
00127                                           float AsocError,
00128                                           int MaxIter, float error_ratio,
00129                                           float error_x, float error_y, float error_t, int IterSmoothConv){
00130 
00131   #ifdef INTMATSM_DEB
00132         printf("-- Init EM params . . ");
00133   #endif
00134 
00135   MAXLASERRANGE = max_laser_range;
00136   params.Bw = Bw;
00137   params.Br = Br*Br;
00138   params.error_th=error_ratio;
00139   params.MaxIter=MaxIter;
00140   params.LMET=L;
00141   params.laserStep=laserStep;
00142   params.MaxDistInter=MaxDistInter;
00143   params.filter=filter;
00144   params.ProjectionFilter=ProjectionFilter;
00145   params.AsocError=AsocError;
00146   params.errx_out=error_x;
00147   params.erry_out=error_y;
00148   params.errt_out=error_t;
00149   params.IterSmoothConv=IterSmoothConv;
00150 
00151   #ifdef INTMATSM_DEB
00152         printf(". OK!\n");
00153   #endif
00154 
00155 }
00156 
00157 
00158 // ************************
00159 // Function that initializes the SM parameters
00160 // ************************
00161 
00162 int MbICPmatcher(Tpfp *laserK, Tpfp *laserK1,
00163                                 Tsc *sensorMotion, Tsc *solution){
00164 
00165         int resEStep=1;
00166         int resMStep=1;
00167         int numIteration=0;
00168 
00169         // Preprocess both scans
00170         preProcessingLib(laserK,laserK1,sensorMotion);
00171 
00172         while (numIteration<params.MaxIter){
00173 
00174                 // Compute the correspondences of the MbICP
00175                 resEStep=EStep();;
00176 
00177                 if (resEStep!=1)
00178                         return -1;
00179 
00180                 // Minize and compute the solution
00181                 resMStep=MStep(solution);
00182 
00183                 if (resMStep==1)
00184                         return 1;
00185                 else if (resMStep==-1)
00186                         return -2;
00187                 else
00188                         numIteration++;
00189         }
00190 
00191         return 2;
00192 
00193 }
00194 
00195 
00196 
00197 // ---------------------------------------------------------------
00198 // ---------------------------------------------------------------
00199 // Inner functions
00200 // ---------------------------------------------------------------
00201 // ---------------------------------------------------------------
00202 
00203 // ************************
00204 // Function that does the association step of the MbICP
00205 // ************************
00206 
00207 static int EStep()
00208 {
00209   int cnt;
00210   int i,J;
00211 
00212   static Tscan ptosNewRef;
00213   static int indexPtosNewRef[MAXLASERPOINTS];
00214 
00215   int L,R,Io;
00216   float dist;
00217   float cp_ass_ptX,cp_ass_ptY,cp_ass_ptD;
00218   float tmp_cp_indD;
00219 
00220   float q1x, q1y, q2x,q2y,p2x,p2y, dqx, dqy, dqpx, dqpy, qx, qy,dx,dy;
00221   float landaMin;
00222   float A,B,C,D;
00223   float LMET2;
00224 
00225   LMET2=params.LMET*params.LMET;
00226 
00227 
00228         // Transform the points according to the current pose estimation
00229 
00230         ptosNewRef.numPuntos=0;
00231         for (i=0; i<ptosNew.numPuntos; i++){
00232                 transfor_directa_p ( ptosNew.laserC[i].x, ptosNew.laserC[i].y,
00233                         &motion2, &ptosNewRef.laserC[ptosNewRef.numPuntos]);
00234                 car2pol(&ptosNewRef.laserC[ptosNewRef.numPuntos],&ptosNewRef.laserP[ptosNewRef.numPuntos]); 
00235                 ptosNewRef.numPuntos++;
00236         }
00237 
00238         // ----
00239         /* Projection Filter */
00240         /* Eliminate the points that cannot be seen */
00241         /* Furthermore it orders the points with the angle */
00242 
00243         cnt = 1; /* Becarefull with this filter (order) when the angles are big >90 */
00244         ptosNoView.numPuntos=0;
00245         if (params.ProjectionFilter==1){
00246                 for (i=1;i<ptosNewRef.numPuntos;i++){
00247                         if (ptosNewRef.laserP[i].t>=ptosNewRef.laserP[cnt-1].t){ 
00248                                 ptosNewRef.laserP[cnt]=ptosNewRef.laserP[i];
00249                                 ptosNewRef.laserC[cnt]=ptosNewRef.laserC[i];
00250                                 cnt++;
00251                         }
00252                         else{
00253                                 ptosNoView.laserP[ptosNoView.numPuntos]=ptosNewRef.laserP[i];
00254                                 ptosNoView.laserC[ptosNoView.numPuntos]=ptosNewRef.laserC[i];
00255                                 ptosNoView.numPuntos++;
00256                         }
00257                 }
00258                 ptosNewRef.numPuntos=cnt;
00259         }
00260 
00261 
00262         // ----
00263         /* Build the index for the windows (this is the role of the Bw parameter */
00264         /* The correspondences are searched between windows in both scans */
00265         /* Like this you speed up the algorithm */
00266 
00267         L=0; R=0; /* index of the window for ptoRef */
00268         Io=0; /* index of the window for ptoNewRef */
00269 
00270         if (ptosNewRef.laserP[Io].t<ptosRef.laserP[L].t) {
00271                 if (ptosNewRef.laserP[Io].t + params.Bw < ptosRef.laserP[L].t){
00272                         while (Io<ptosNewRef.numPuntos-1 && ptosNewRef.laserP[Io].t + params.Bw < ptosRef.laserP[L].t) {
00273                                 Io++;
00274                         }
00275                 }
00276                 else{
00277                         while (R<ptosRef.numPuntos-1 && ptosNewRef.laserP[Io].t + params.Bw > ptosRef.laserP[R+1].t)
00278                                 R++;
00279                 }
00280         }
00281         else{
00282                 while (L<ptosRef.numPuntos-1 && ptosNewRef.laserP[Io].t - params.Bw > ptosRef.laserP[L].t)
00283                         L++;
00284                 R=L;
00285                 while (R<ptosRef.numPuntos-1 && ptosNewRef.laserP[Io].t + params.Bw > ptosRef.laserP[R+1].t)
00286                         R++;
00287         }
00288 
00289         // ----
00290         /* Look for potential correspondences between the scans */
00291         /* Here is where we use the windows */
00292 
00293         cnt=0;
00294         for (i=Io;i<ptosNewRef.numPuntos;i++){
00295 
00296                 // Keep the index of the original scan ordering
00297                 cp_associations[cnt].index=indexPtosNewRef[i];
00298 
00299                 // Move the window
00300                 while  (L < ptosRef.numPuntos-1 && ptosNewRef.laserP[i].t - params.Bw > ptosRef.laserP[L].t)
00301                         L = L + 1;
00302                 while (R <ptosRef.numPuntos-1 && ptosNewRef.laserP[i].t + params.Bw > ptosRef.laserP[R+1].t)
00303                         R = R + 1;
00304 
00305                 cp_associations[cnt].L=L;
00306                 cp_associations[cnt].R=R;
00307 
00308                 if (L==R){
00309                         // Just one possible correspondence
00310 
00311                         // precompute stuff to speed up
00312                         qx=ptosRef.laserC[R].x; qy=ptosRef.laserC[R].y;
00313                         p2x=ptosNewRef.laserC[i].x;     p2y=ptosNewRef.laserC[i].y;
00314                         dx=p2x-qx; dy=p2y-qy;
00315                         dist=dx*dx+dy*dy-(dx*qy-dy*qx)*(dx*qy-dy*qx)/(qx*qx+qy*qy+LMET2);
00316 
00317                         if (dist<params.Br){
00318                                 cp_associations[cnt].nx=ptosNewRef.laserC[i].x;
00319                                 cp_associations[cnt].ny=ptosNewRef.laserC[i].y;
00320                                 cp_associations[cnt].rx=ptosRef.laserC[R].x;
00321                                 cp_associations[cnt].ry=ptosRef.laserC[R].y;
00322                                 cp_associations[cnt].dist=dist;
00323                                 cnt++;
00324                         }
00325                 }
00326                 else if (L<R)
00327                 {
00328                         // More possible correspondences
00329 
00330                         cp_ass_ptX=0;
00331                         cp_ass_ptY=0;
00332                         cp_ass_ptD=100000;
00333 
00334                         /* Metric based Closest point rule */
00335                         for (J=L+1;J<=R;J++){
00336 
00337                                 // Precompute stuff to speed up
00338                                 q1x=ptosRef.laserC[J-1].x; q1y=ptosRef.laserC[J-1].y;
00339                                 q2x=ptosRef.laserC[J].x; q2y=ptosRef.laserC[J].y;
00340                                 p2x=ptosNewRef.laserC[i].x; p2y=ptosNewRef.laserC[i].y;
00341 
00342                                 dqx=refdqx[J-1]; dqy=refdqy[J-1];
00343                                 dqpx=q1x-p2x;  dqpy=q1y-p2y;
00344                                 A=1/(p2x*p2x+p2y*p2y+LMET2);
00345                                 B=(1-A*p2y*p2y);
00346                                 C=(1-A*p2x*p2x);
00347                                 D=A*p2x*p2y;
00348 
00349                                 landaMin=(D*(dqx*dqpy+dqy*dqpx)+B*dqx*dqpx+C*dqy*dqpy)/(B*refdqx2[J-1]+C*refdqy2[J-1]+2*D*refdqxdqy[J-1]);
00350 
00351                                 if (landaMin<0){ // Out of the segment on one side
00352                                         qx=q1x; qy=q1y;}
00353                                 else if (landaMin>1){ // Out of the segment on the other side
00354                                         qx=q2x; qy=q2y;}
00355                                 else if (distref[J-1]<params.MaxDistInter) { // Within the segment and interpotation OK
00356                                         qx=(1-landaMin)*q1x+landaMin*q2x;
00357                                         qy=(1-landaMin)*q1y+landaMin*q2y;
00358                                 }
00359                                 else{ // Segment too big do not interpolate
00360                                         if (landaMin<0.5){
00361                                                 qx=q1x; qy=q1y;}
00362                                         else{
00363                                                 qx=q2x; qy=q2y;}
00364                                 }
00365 
00366                                 // Precompute stuff to see if we save the association
00367                                 dx=p2x-qx;
00368                                 dy=p2y-qy;
00369                                 tmp_cp_indD=dx*dx+dy*dy-(dx*qy-dy*qx)*(dx*qy-dy*qx)/(qx*qx+qy*qy+LMET2);
00370 
00371                                 // Check if the association is the best up to now
00372                                 if (tmp_cp_indD < cp_ass_ptD){
00373                                         cp_ass_ptX=qx;
00374                                         cp_ass_ptY=qy;
00375                                         cp_ass_ptD=tmp_cp_indD;
00376                                 }
00377                         }
00378 
00379                         // Association compatible in distance (Br parameter)
00380                         if (cp_ass_ptD< params.Br){
00381                                 cp_associations[cnt].nx=ptosNewRef.laserC[i].x;
00382                                 cp_associations[cnt].ny=ptosNewRef.laserC[i].y;
00383                                 cp_associations[cnt].rx=cp_ass_ptX;
00384                                 cp_associations[cnt].ry=cp_ass_ptY;
00385                                 cp_associations[cnt].dist=cp_ass_ptD;
00386 
00387                                 cnt++;
00388                         }
00389                 }
00390                 else { // This cannot happen but just in case ...
00391                         cp_associations[cnt].nx=ptosNewRef.laserC[i].x;
00392                         cp_associations[cnt].ny=ptosNewRef.laserC[i].y;
00393                         cp_associations[cnt].rx=0;
00394                         cp_associations[cnt].ry=0;
00395                         cp_associations[cnt].dist=params.Br;
00396                         cnt++;
00397                 }
00398         }  // End for (i=Io;i<ptosNewRef.numPuntos;i++){
00399 
00400         cntAssociationsT=cnt;
00401 
00402         // Check if the number of associations is ok
00403         if (cntAssociationsT<ptosNewRef.numPuntos*params.AsocError){
00404                 #ifdef INTMATSM_DEB
00405                         printf("Number of associations too low <%d out of %f>\n",
00406                                 cntAssociationsT,ptosNewRef.numPuntos*params.AsocError);
00407                 #endif
00408                 return 0;
00409         }
00410 
00411         return 1;
00412 }
00413 
00414 
00415 // ************************
00416 // Function that does the minimization step of the MbICP
00417 // ************************
00418 
00419 static int MStep(Tsc *solucion){
00420 
00421   Tsc estim_cp;
00422   int i,cnt,res;
00423   float error_ratio, error;
00424   float cosw, sinw, dtx, dty, tmp1, tmp2;
00425   static TAsoc cp_tmp[MAXLASERPOINTS+1];
00426 
00427         // Filtering of the spurious data
00428         // Used the trimmed versions that orders the point by distance between associations
00429 
00430      if (params.filter<1){
00431 
00432                 // Add Null element in array position 0 (this is because heapsort requirement)
00433                 for (i=0;i<cntAssociationsT;i++){
00434                         cp_tmp[i+1]=cp_associations[i];
00435                 }
00436                 cp_tmp[0].dist=-1;
00437                 // Sort array
00438                 heapsort(cp_tmp, cntAssociationsT);
00439                 // Filter out big distances
00440                 cnt=((int)(cntAssociationsT*100*params.filter))/100;
00441                 // Remove Null element
00442                 for (i=0;i<cnt;i++){
00443                         cp_associationsTemp[i]=cp_tmp[i+1];
00444                 }
00445          }
00446          else{ // Just build the Temp array to minimize
00447                 cnt=0;
00448                 for (i=0; i<cntAssociationsT;i++){
00449                         if (cp_associations[i].dist<params.Br){
00450                                 cp_associationsTemp[cnt]=cp_associations[i];
00451                                 cnt++;
00452                         }
00453                 }
00454         }
00455 
00456         cntAssociationsTemp=cnt;
00457 
00458         #ifdef INTMATSM_DEB
00459                 printf("All assoc: %d  Filtered: %d  Percentage: %f\n",
00460                         cntAssociationsT, cntAssociationsTemp, cntAssociationsTemp*100.0/cntAssociationsT);
00461         #endif
00462 
00463         // ---
00464         /* Do de minimization Minimize Metric-based distance */
00465         /* This function is optimized to speed up */
00466 
00467         res=computeMatrixLMSOpt(cp_associationsTemp,cnt,&estim_cp);
00468         if (res==-1)
00469                 return -1;
00470 
00471         #ifdef INTMATSM_DEB
00472                 printf("estim_cp: <%f %f %f>\n",estim_cp.x, estim_cp.y,estim_cp.tita);
00473                 printf("New impl: <%f %f %f>\n",estim_cp.x, estim_cp.y,estim_cp.tita);
00474         #endif
00475 
00476         cosw=(float)cos(estim_cp.tita); sinw=(float)sin(estim_cp.tita);
00477         dtx=estim_cp.x; dty=estim_cp.y;
00478 
00479 
00480         // ------
00481         /* Compute the error of the associations */
00482 
00483         error=0;
00484         for (i = 0; i<cnt;i++){
00485                 tmp1=cp_associationsTemp[i].nx * cosw - cp_associationsTemp[i].ny * sinw + dtx - cp_associationsTemp[i].rx;tmp1*=tmp1;
00486                 tmp2=cp_associationsTemp[i].nx * sinw + cp_associationsTemp[i].ny * cosw + dty - cp_associationsTemp[i].ry;tmp2*=tmp2;
00487                 error = error+ tmp1+tmp2;
00488         }
00489 
00490         error_ratio = error / error_k1;
00491 
00492         #ifdef INTMATSM_DEB
00493                 printf("<err,errk1,errRatio>=<%f,%f,%f>\n estim=<%f,%f,%f>\n",
00494                         error,error_k1,error_ratio, estim_cp.x,estim_cp.y, estim_cp.tita);
00495         #endif
00496 
00497         // ----
00498         /* Check the exit criteria */
00499         /* Error ratio */
00500         if (fabs(1.0-error_ratio)<=params.error_th ||
00501                 (fabs(estim_cp.x)<params.errx_out && fabs(estim_cp.y)<params.erry_out
00502                 && fabs(estim_cp.tita)<params.errt_out) ){
00503                 numConverged++;
00504         }
00505         else
00506                 numConverged=0;
00507 
00508         //--
00509         /* Build the solution */
00510         composicion_sis(&estim_cp, &motion2, solucion);
00511         motion2=*solucion;
00512         error_k1=error;
00513 
00514         /* Number of iterations doing convergence (smooth criterion of convergence) */
00515         if (numConverged>params.IterSmoothConv)
00516                 return 1;
00517         else
00518                 return 0;
00519 }
00520 
00521 
00522 // ************************
00523 // Function to do the least-squares but optimized for the metric
00524 // ************************
00525 
00526 static int computeMatrixLMSOpt(TAsoc *cp_ass, int cnt, Tsc *estimacion) {
00527 
00528         int i;
00529         float LMETRICA2;
00530         float X1[MAXLASERPOINTS], Y1[MAXLASERPOINTS];
00531         float X2[MAXLASERPOINTS],Y2[MAXLASERPOINTS];
00532         float X2Y2[MAXLASERPOINTS],X1X2[MAXLASERPOINTS];
00533         float X1Y2[MAXLASERPOINTS], Y1X2[MAXLASERPOINTS];
00534         float Y1Y2[MAXLASERPOINTS];
00535         float K[MAXLASERPOINTS], DS[MAXLASERPOINTS];
00536         float DsD[MAXLASERPOINTS], X2DsD[MAXLASERPOINTS], Y2DsD[MAXLASERPOINTS];
00537         float Bs[MAXLASERPOINTS], BsD[MAXLASERPOINTS];
00538         float A1, A2, A3, B1, B2, B3, C1, C2, C3, D1, D2, D3;
00539         MATRIX matA,invMatA;
00540         VECTOR vecB,vecSol;
00541 
00542         A1=0;A2=0;A3=0;B1=0;B2=0;B3=0;
00543         C1=0;C2=0;C3=0;D1=0;D2=0;D3=0;
00544 
00545 
00546         LMETRICA2=params.LMET*params.LMET;
00547 
00548         for (i=0; i<cnt; i++){
00549                 X1[i]=cp_ass[i].nx*cp_ass[i].nx;
00550                 Y1[i]=cp_ass[i].ny*cp_ass[i].ny;
00551                 X2[i]=cp_ass[i].rx*cp_ass[i].rx;
00552                 Y2[i]=cp_ass[i].ry*cp_ass[i].ry;
00553                 X2Y2[i]=cp_ass[i].rx*cp_ass[i].ry;
00554 
00555                 X1X2[i]=cp_ass[i].nx*cp_ass[i].rx;
00556                 X1Y2[i]=cp_ass[i].nx*cp_ass[i].ry;
00557                 Y1X2[i]=cp_ass[i].ny*cp_ass[i].rx;
00558                 Y1Y2[i]=cp_ass[i].ny*cp_ass[i].ry;
00559 
00560                 K[i]=X2[i]+Y2[i] + LMETRICA2;
00561                 DS[i]=Y1Y2[i] + X1X2[i];
00562                 DsD[i]=DS[i]/K[i];
00563                 X2DsD[i]=cp_ass[i].rx*DsD[i];
00564                 Y2DsD[i]=cp_ass[i].ry*DsD[i];
00565 
00566                 Bs[i]=X1Y2[i]-Y1X2[i];
00567                 BsD[i]=Bs[i]/K[i];
00568 
00569                 A1=A1 + (1-Y2[i]/K[i]);
00570                 B1=B1 + X2Y2[i]/K[i];
00571                 C1=C1 + (-cp_ass[i].ny + Y2DsD[i]);
00572                 D1=D1 + (cp_ass[i].nx - cp_ass[i].rx -cp_ass[i].ry*BsD[i]);
00573 
00574                 A2=B1;
00575                 B2=B2 + (1-X2[i]/K[i]);
00576                 C2=C2 + (cp_ass[i].nx-X2DsD[i]);
00577                 D2=D2 + (cp_ass[i].ny -cp_ass[i].ry +cp_ass[i].rx*BsD[i]);
00578 
00579                 A3=C1;
00580                 B3=C2;
00581                 C3=C3 + (X1[i] + Y1[i] - DS[i]*DS[i]/K[i]);
00582                 D3=D3 + (Bs[i]*(-1+DsD[i]));
00583         }
00584 
00585 
00586         initialize_matrix(&matA,3,3);
00587         MDATA(matA,0,0)=A1;     MDATA(matA,0,1)=B1;     MDATA(matA,0,2)=C1;
00588         MDATA(matA,1,0)=A2;     MDATA(matA,1,1)=B2;     MDATA(matA,1,2)=C2;
00589         MDATA(matA,2,0)=A3;     MDATA(matA,2,1)=B3;     MDATA(matA,2,2)=C3;
00590 
00591         if (inverse_matrix (&matA, &invMatA)==-1)
00592                 return -1;
00593 
00594 #ifdef INTMATSM_DEB
00595         print_matrix("inverted matrix", &invMatA);
00596 #endif
00597 
00598         initialize_vector(&vecB,3);
00599         VDATA(vecB,0)=D1; VDATA(vecB,1)=D2; VDATA(vecB,2)=D3;
00600         multiply_matrix_vector (&invMatA, &vecB, &vecSol);
00601 
00602         estimacion->x=-VDATA(vecSol,0);
00603         estimacion->y=-VDATA(vecSol,1);
00604         estimacion->tita=-VDATA(vecSol,2);
00605 
00606         return 1;
00607 }
00608 
00609 
00610 // ------------------------------------
00611 // Function added by Javi for compatibility
00612 // ------------------------------------
00613 
00614 static void preProcessingLib(Tpfp *laserK, Tpfp *laserK1,
00615                                           Tsc *initialMotion)
00616 {
00617 
00618         int i,j;
00619 
00620         motion2=*initialMotion;
00621 
00622         // ------------------------------------------------//
00623         // Compute xy coordinates of the points in laserK1
00624         ptosNew.numPuntos=0;
00625         for (i=0; i<MAXLASERPOINTS; i++) {
00626                 if (laserK1[i].r <MAXLASERRANGE){
00627                         ptosNew.laserP[ptosNew.numPuntos].r=laserK1[i].r;
00628                         ptosNew.laserP[ptosNew.numPuntos].t=laserK1[i].t;
00629                         ptosNew.laserC[ptosNew.numPuntos].x=(float)(laserK1[i].r * cos(laserK1[i].t));
00630                         ptosNew.laserC[ptosNew.numPuntos].y=(float)(laserK1[i].r * sin(laserK1[i].t));
00631                     ptosNew.numPuntos++;
00632                 }
00633         }
00634 
00635         // Choose one point out of params.laserStep points
00636         j=0;
00637         for (i=0; i<ptosNew.numPuntos; i+=params.laserStep) {
00638                 ptosNew.laserC[j]=ptosNew.laserC[i];
00639                 j++;
00640         }
00641         ptosNew.numPuntos=j;
00642 
00643         // Compute xy coordinates of the points in laserK
00644         ptosRef.numPuntos=0;
00645         for (i=0; i<MAXLASERPOINTS; i++) {
00646                 if (laserK[i].r <MAXLASERRANGE){
00647                         ptosRef.laserP[ptosRef.numPuntos].r=laserK[i].r;
00648                         ptosRef.laserP[ptosRef.numPuntos].t=laserK[i].t;
00649                         ptosRef.laserC[ptosRef.numPuntos].x=(float)(laserK[i].r * cos(laserK1[i].t));
00650                         ptosRef.laserC[ptosRef.numPuntos].y=(float)(laserK[i].r * sin(laserK1[i].t));
00651                     ptosRef.numPuntos++;
00652                 }
00653         }
00654 
00655         // Choose one point out of params.laserStep points
00656         j=0;
00657         for (i=0; i<ptosRef.numPuntos; i+=params.laserStep) {
00658                 ptosRef.laserC[j]=ptosRef.laserC[i];
00659                 j++;
00660         }
00661         ptosRef.numPuntos=j;
00662         // ------------------------------------------------//
00663 
00664         // Preprocess reference points
00665         for (i=0;i<ptosRef.numPuntos-1;i++) {
00666                 car2pol(&ptosRef.laserC[i],&ptosRef.laserP[i]);
00667                 refdqx[i]=ptosRef.laserC[i].x - ptosRef.laserC[i+1].x;
00668                 refdqy[i]=ptosRef.laserC[i].y - ptosRef.laserC[i+1].y;
00669                 refdqx2[i]=refdqx[i]*refdqx[i];
00670                 refdqy2[i]=refdqy[i]*refdqy[i];
00671                 distref[i]=refdqx2[i] + refdqy2[i];
00672                 refdqxdqy[i]=refdqx[i]*refdqy[i];
00673         }
00674         car2pol(&ptosRef.laserC[ptosRef.numPuntos-1],&ptosRef.laserP[ptosRef.numPuntos-1]);
00675 
00676         error_k1=BIG_INITIAL_ERROR;
00677         numConverged=0;
00678 }


csm
Author(s): Andrea Censi
autogenerated on Mon Jan 16 2017 03:48:29