|
MRXT: The Multi-Robot eXploration Tool
Multi-Robot autonomous exploration and mapping simulator.
|
|
MRXT: The Multi-Robot eXploration Tool
Multi-Robot autonomous exploration and mapping simulator.
|
00001 #include "EKFilter.h" 00002 #include <opencv2/opencv.hpp> 00003 #include <list> 00004 00005 #ifndef WIN32 00006 #include "sys/time.h" 00007 #endif 00008 00009 using namespace std; 00010 00011 // Para registrar la clase en la factoria 00012 namespace{ 00013 slamInterface* CreateEKFilter(int bots, ConfigFile& file){ 00014 return new EKFilter(bots, file); 00015 } 00016 const int EKF = 1; 00017 const bool registered = slamFactory::Instance().Register(EKF, slamCreator(CreateEKFilter)); 00018 } 00019 00020 /// Constructor 00021 EKFilter::EKFilter(): 00022 vslamFilter(), 00023 robotcells(0), 00024 dispRobot(0), 00025 odoData(0), 00026 lastOdoData(0), 00027 rsData(0), 00028 lmData(0), 00029 endSlam(false), 00030 omap(0), 00031 ppmap(0), 00032 ipmap(0), 00033 vmap(0), 00034 Identity3(matrix::identity(3)), 00035 maxCorrect(0.0f), 00036 minIncorrect(999999999.9f) 00037 { 00038 //printf("[EKF] EKF default constructor\n"); 00039 } 00040 00041 /// Constructor 00042 EKFilter::EKFilter(int nrobots, ConfigFile& configFile): 00043 vslamFilter (nrobots,configFile), 00044 state (3*nrobots,1,3*(nrobots+nmarks),1), 00045 covariance (3*nrobots,3*nrobots,3*(nrobots+nmarks),3*(nrobots+nmarks)), 00046 robotcells (new point[nBots]), 00047 dispRobot (new float[nBots]), 00048 odoData (new pose*[nBots]), 00049 lastOdoData (new pose*[nBots]), 00050 rsData (new rangeSensorData*[nBots]), 00051 lmData (new landmarksData*[nBots]), 00052 endSlam (false), 00053 omap (gridMapFactory::Instance().CreateObject(gmtype,width, height, resolution, xorigin, yorigin)), 00054 ppmap (new binMap(gmwidth, gmheight, resolution, xorigin, yorigin)), 00055 ipmap (new binMap(gmwidth, gmheight, resolution, xorigin, yorigin)), 00056 vmap (visualMapFactory::Instance().CreateObject(vmtype,nmarks,configFile)), 00057 Identity3 (matrix::identity(3)), 00058 // range (configFile.read<float>("SEARCHRANGE")), 00059 mahTh (configFile.read<float>("MAHALANOBISTH")), 00060 descTh (configFile.read<float>("DESCRIPTORTH")), 00061 nearestNeighbourOrderBy (configFile.read<int>("NEARESTNEIGHBOURBY")), 00062 maxCorrect (0.0f), 00063 minIncorrect (999999999.9f) 00064 { 00065 for (int r = 0; r< nBots; r++){ 00066 dispRobot[r] = 0.0f; 00067 rbase[r] = 0; 00068 odoData[r] = 0; 00069 lastOdoData[r] = 0; 00070 rsData[r] = 0; 00071 lmData[r] = 0; 00072 robotsEnabled[r]=true; 00073 } 00074 printf("[EKF] EKF created\n"); 00075 } 00076 00077 /// Destructor 00078 EKFilter::~EKFilter(){ 00079 //printf("[EKF] EKF destroyer..."); 00080 stop(); 00081 if (logstr){ 00082 char myfilestr[100]; 00083 sprintf(myfilestr,"%somap.jpg",logstr); 00084 omap->saveMapAsImage(myfilestr); 00085 sprintf(myfilestr,"%sppmap.jpg",logstr); 00086 ppmap->saveMapAsImage(myfilestr); 00087 sprintf(myfilestr,"%svmap",logstr); 00088 vmap->saveMap(myfilestr); 00089 } 00090 if (robotcells) delete[] robotcells; 00091 if (dispRobot) delete[] dispRobot; 00092 if (odoData){ for (int i=0; i< nBots; i++) if (odoData[i]) delete odoData[i]; 00093 delete[] odoData; 00094 } 00095 if (lastOdoData){ for (int i=0; i< nBots; i++) if (lastOdoData[i]) delete lastOdoData[i]; 00096 delete[] lastOdoData; 00097 } 00098 if (rsData){ for (int i=0; i< nBots; i++) if (rsData[i]) delete rsData[i]; 00099 delete[] rsData;} 00100 if (lmData){ for (int i=0; i< nBots; i++) if (lmData[i]) delete lmData[i]; 00101 delete[] lmData;} 00102 if (omap) delete omap; 00103 if (ppmap) delete ppmap; 00104 if (ipmap) delete ipmap; 00105 if (vmap) delete vmap; 00106 //printf("EKF destroyed\n"); 00107 } 00108 00109 ///initializer 00110 void EKFilter::initialize(int nrobots, ConfigFile& configfile){ 00111 vslamFilter::initialize(nrobots, configfile); 00112 00113 state.initialize(3*nrobots,1,3*(nrobots+nmarks),1); 00114 covariance.initialize(3*nrobots,3*nrobots,3*(nrobots+nmarks),3*(nrobots+nmarks)); 00115 00116 if (robotcells) delete[] robotcells; 00117 robotcells = new point[nBots]; 00118 if (dispRobot) delete[] dispRobot; 00119 dispRobot = new float[nBots]; 00120 if (odoData) delete[] odoData; 00121 odoData = new pose*[nBots]; 00122 if (lastOdoData) delete[] lastOdoData; 00123 lastOdoData = new pose*[nBots]; 00124 if (rsData) delete[] rsData; 00125 rsData = new rangeSensorData*[nBots]; 00126 if (lmData) delete[] lmData; 00127 lmData = new landmarksData*[nBots]; 00128 endSlam = false; 00129 if (omap) delete omap; 00130 omap = gridMapFactory::Instance().CreateObject(gmtype, width, height, resolution, xorigin, yorigin); 00131 if (ppmap) delete ppmap; 00132 ppmap = new binMap(gmwidth, gmheight, resolution, xorigin, yorigin); 00133 if (ipmap) delete ipmap; 00134 ipmap = new binMap(gmwidth, gmheight, resolution, xorigin, yorigin); 00135 if (vmap) delete vmap; 00136 vmap = visualMapFactory::Instance().CreateObject(vmtype,nmarks,configfile); 00137 00138 for (int r = 0; r< nBots; r++){ 00139 dispRobot[r] = 0.0f; 00140 rbase[r] = 0; 00141 odoData[r] = 0; 00142 lastOdoData[r] = 0; 00143 rsData[r] = 0; 00144 lmData[r] = 0; 00145 robotsEnabled[r]=true; 00146 } 00147 } 00148 00149 /// set up for the execution thread 00150 int EKFilter::setup(){ 00151 prio = 50; 00152 return 0; 00153 } 00154 00155 /// called before stopping 00156 void EKFilter::onStop(){ 00157 if (!endSlam){ 00158 endSlam = true; 00159 //scene->beginProduction(); 00160 //scene->endProduction(); 00161 closing.lock(); 00162 closing.unlock(); 00163 } 00164 printf("[EKF] SLAM stopped\n"); 00165 } 00166 00167 /// primary execution loop 00168 void EKFilter::execute(){ 00169 closing.lock(); 00170 printf("[EKF] \t\t\t\t\t\t\t Running SLAM\n"); 00171 endSlam=false; 00172 int r; 00173 00174 // open the log file 00175 FILE* slamlog=0; 00176 CvVideoWriter* avifile=0; 00177 00178 if (logstr){ 00179 char myfilestr[100]; 00180 sprintf(myfilestr,"%sslam.m",logstr); 00181 slamlog = fopen(myfilestr,"w"); 00182 fprintf(slamlog,"poses_est=["); 00183 00184 // if (saveAviFile){ 00185 // sprintf(myfilestr,"outfiles/%svideo.avi",logstr); 00186 // avifile = cvCreateVideoWriter( myfilestr, -1, 25, cvSize(gmwidth,gmheight),1); 00187 // //printf("[EKF] avifile created\n"); 00188 // } 00189 } 00190 00191 // if (displayomap){ 00192 // if (windowName) cvNamedWindow(windowName,0); 00193 // else cvNamedWindow("OMAP",0); 00194 // } 00195 // if (displayppmap) cvNamedWindow("PPMAP", 0); 00196 // if (displayipmap) cvNamedWindow("IPMAP", 0); 00197 00198 // inicializamos SLAM 00199 for (r=0; r<nBots; r++){ 00200 if (rbase[r]) rbase[r]->beginConsumition(); 00201 odoData[r] = rbase[r]->getOdometry(); 00202 state.set(r*3,0,odoData[r]->x); 00203 state.set(r*3+1,0,odoData[r]->y); 00204 state.set(r*3+2,0,odoData[r]->th); 00205 pose pos = getPos(r); 00206 robotcells[r].x = (int) floor((pos.x - xorigin)/resolution); 00207 robotcells[r].y = (int) floor((pos.y - yorigin)/resolution); 00208 if (rbase[r]) rbase[r]->endConsumition(); 00209 } 00210 //state.print("initial state"); 00211 covariance.clear(); 00212 // #ifndef WIN32 00213 // struct timeval tv1, tv2; 00214 // double initime, curtime, time1, time2, time3; 00215 // #endif 00216 // 00217 int paso = 0; 00218 int nBotsx3 = 3*nBots; 00219 matrix Ft = matrix::identity(nBotsx3); 00220 Ematrix Qt(nBotsx3,nBotsx3); 00221 00222 00223 errorcount = 0; 00224 while (!endSlam){ 00225 //printID("SLAM EKF"); 00226 paso++; 00227 // Leemos sensores 00228 //printf("[EKF] \t\t\tslam step\n"); 00229 for (r=0; r<nBots; r++){ 00230 if (robotsEnabled[r]){ 00231 //printf("[EKF][robot %d] waiting to consume data...\n", r); 00232 rbase[r]->beginConsumition(); 00233 if (robotsEnabled[r]){ 00234 if (lastOdoData[r]) delete lastOdoData[r]; 00235 lastOdoData[r] = odoData[r]; 00236 odoData[r] = rbase[r]->getOdometry(); 00237 if (rsData[r]) delete rsData[r]; 00238 rsData[r] = rbase[r]->getRangeSensorData(); 00239 if (lmData[r]) delete lmData[r]; 00240 lmData[r] = rbase[r]->getLandmarksData(); 00241 } 00242 //printf("[EKF][robot %d] data consumed\n", r); 00243 rbase[r]->endConsumition(); 00244 } 00245 } 00246 //printf("[EKF] \t\t\tconsumed\n"); 00247 00248 // #ifndef WIN32 00249 // gettimeofday(&tv1, NULL); 00250 // initime = tv1.tv_sec*1000.0 + tv1.tv_usec/1000.0; 00251 // //printf("init time = %f\n",initime); 00252 // #endif 00253 00254 //sleepms(2000); 00255 00256 int nmarksprev = vmap->getNLandmarks(); 00257 int nmarksprev3 = 3*nmarksprev; 00258 00259 Ematrix G(nBotsx3+nmarksprev3,2); 00260 Ematrix& xpkmas1 = state; 00261 00262 // #ifndef WIN32 00263 // gettimeofday(&tv2, NULL); 00264 // time1 = tv2.tv_sec*1000.0 + tv2.tv_usec/1000.0; 00265 // printf("Prediction Time: %f \n", time1-initime); 00266 // #endif 00267 00268 int nummarks=0; 00269 for (r=0; r<nBots; r++){ 00270 if (robotsEnabled[r]){ 00271 int r3 = 3*r; 00272 // pose 00273 pose xrk = getPos(r); // cogemos la pose del estado 00274 // odo inc 00275 pose deltaOdo = *(odoData[r]) - *(lastOdoData[r]); // incremento de odo 00276 if (deltaOdo.th>PI) deltaOdo.th -= PIx2; 00277 else if (deltaOdo.th <= -PI) deltaOdo.th += PIx2; 00278 00279 // #ifndef WIN32 00280 // gettimeofday(&tv2, NULL); 00281 // time1 = tv2.tv_sec*1000.0 + tv2.tv_usec/1000.0; 00282 // printf("Prediction Time: %f \n", time1-initime); 00283 00284 00285 // #endif 00286 00287 //deltaOdo.print("\t\t\t\t\t\t\t\t\tdeltaOdo="); 00288 // prediction 00289 Ft.set(r3,r3,jacobianF(xrk, deltaOdo,lastOdoData[r]->th)); // ampliamos el jacobiano F 00290 Ematrix G = jacobianG(xrk, deltaOdo,lastOdoData[r]->th); // jacobiano G 00291 Qt.set(r3,r3,G*(noiseQ(xrk,deltaOdo,lastOdoData[r]->th)*G.transpose())); 00292 00293 xpkmas1.set(r3, 0, motionModel (xrk, deltaOdo, lastOdoData[r]->th)); // modelo de movimiento 00294 nummarks += lmData[r]->getNLandmarks(); 00295 //if (lmData[r]->getNLandmarks() < 3) printf("[EKF] [robot %d] >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> OJO MARCAS MENOR QUE 3!!!!!! solo %d\n", r, lmData[r]->getNLandmarks()); 00296 } 00297 } 00298 // #ifndef WIN32 00299 // gettimeofday(&tv2, NULL); 00300 // time1 = tv2.tv_sec*1000.0 + tv2.tv_usec/1000.0; 00301 //printf("Prediction Time: %f \n", time1-initime); 00302 // #endif 00303 00304 // covariance.print("P="); 00305 00306 // F.print("F="); 00307 // Ft.print("Ft="); 00308 00309 // Q.print("Q="); 00310 // Qt.print("Qt="); 00311 00312 // Ematrix Ppkmas1 = F * (covariance * F.transpose()) +Q; // matriz de covarianza 00313 // Ppkmas1.print("completa\n"); 00314 00315 Ematrix& Ppkmas1 = covariance; 00316 matrix Fttrans = Ft.transpose(); 00317 matrix Pvv = Ppkmas1.subMat(0,nBotsx3-1,0,nBotsx3-1); 00318 // Pvv.print("Pvv="); 00319 Ppkmas1.set(0,0,Qt + ( Ft* (Pvv*Fttrans) )); 00320 matrix Pvl = Ppkmas1.subMat(0,nBotsx3-1,nBotsx3,nBotsx3+nmarksprev3-1); 00321 // Pvl.print("Pvl="); 00322 Ppkmas1.set(0,nBotsx3,Ft*Pvl); 00323 Ppkmas1.set(nBotsx3,0,(Ppkmas1.subMat(0,nBotsx3-1,nBotsx3,nBotsx3+nmarksprev3-1)).transpose()); 00324 00325 //Ppkmas1.print("Predicted P\n"); 00326 00327 //Ppkmas1 = Ppkmas1t; 00328 00329 // #ifndef WIN32 00330 // gettimeofday(&tv2, NULL); 00331 // time1 = tv2.tv_sec*1000.0 + tv2.tv_usec/1000.0; 00332 // //printf("[EKF] Prediction Time: %f \n", time1-initime); 00333 // #endif 00334 00335 // update 00336 00337 int nummarks3 = 3*nummarks; 00338 00339 Ematrix H(0,nBotsx3+nmarksprev3,3*nummarks,nBotsx3+nmarksprev3+nummarks3); 00340 Ematrix ZT(0,1,nummarks3,1); 00341 Ematrix zgorro(0,1,nummarks3,1); 00342 Ematrix Rt(0,0,nummarks3,nummarks3); 00343 00344 int markid=0; 00345 00346 for (r=0; r<nBots; r++){ 00347 if (robotsEnabled[r]){ 00348 int r3 = 3*r; 00349 int r3mas2 = r3+2; 00350 pose xrpkmas1 = xpkmas1.subMat(r3,r3mas2,0,0).toPose(); 00351 matrix H2 = jacobianH2(xrpkmas1); // jacobiano H 00352 matrix H2trans = H2.transpose(); 00353 //xrpkmas1.print("Xk|k-1 = "); 00354 //printf("numero de observaciones= %d\n",lmData[r]->getNLandmarks()); 00355 //if (lmData[r]->getNLandmarks()>2){ 00356 for (int j = 0; j < lmData[r]->getNLandmarks(); j++){ // for each detected mark 00357 //printf("Marca vista con descriptor: %f\n", lmData[r]->getLandmark(j).descriptor[0]); 00358 pos3d lpos = base2global(vmap->cam2base(lmData[r]->getLandmark(j).pos), xrpkmas1); // medida en coordenadas globales 00359 matrix ZTi = lmData[r]->getLandmark(j).pos; // Medida (coordenadas de camara) 00360 matrix Rti = lmData[r]->getLandmark(j).covariance; // covarianza de la medida 00361 //ZTi.print("Observacion = "); 00362 //Rti.print("RTi="); 00363 // data association 00364 00365 landmark* mark = (perfectMatching)? 00366 vmap->getLandmarkByDesc(lmData[r]->getLandmark(j).descriptor) : 00367 // vmap->dataAssociation(ZTi, Rti, lpos, xrpkmas1, H2, H2trans, lmData[r]->getLandmark(j).descriptor) ; 00368 dataAssociation(ZTi, Rti, lpos, xrpkmas1, H2, H2trans, lmData[r]->getLandmark(j).descriptor,Ppkmas1,r) ; 00369 00370 if (mark){ // si hay matching 00371 00372 //printf("matching con la marca %f\n",mark->descriptor[0]); 00373 00374 int markid3 = 3*markid; 00375 ZT.extend(3,0); 00376 ZT.set(markid3,0,ZTi); 00377 Rt.extend(3,3); 00378 Rt.set(markid3,markid3,Rti); 00379 matrix zgorroi = vmap->base2cam(global2base(mark->pos, xrpkmas1)); // medida estimada 00380 zgorro.extend(3,0); 00381 zgorro.set(markid3,0,zgorroi); 00382 00383 matrix H1= jacobianH1(xrpkmas1,mark->pos); 00384 int lid = vmap->returnLastReturnedId(); 00385 H.extend(3,0); 00386 H.set(markid3,nBotsx3+3*lid,H2); 00387 H.set(markid3,r3,H1); 00388 00389 markid++; 00390 00391 for (int k=0; k< mark->desclength; k++) 00392 mark->descriptor[k] = (mark->descriptor[k] + lmData[r]->getLandmark(j).descriptor[k])/2; 00393 vmap->changeLastReturnedLandmark(*mark); 00394 } 00395 else{ // new mark 00396 //printf("marca nueva\n"); 00397 matrix Pl2 = H2trans*(Rti*(H2)); 00398 //Rti.print("RTi="); 00399 mark = new landmark(lpos, // global pos 00400 Pl2, // covariance 00401 lmData[r]->getLandmark(j).descriptor, // descriptor 00402 lmData[r]->getLandmark(j).desclength, // descriptor length 00403 0); // repeated 00404 vmap->addLandmark(*mark); 00405 00406 int numm = 3*(vmap->getNLandmarks()-1); 00407 00408 // add the mark to the state map 00409 xpkmas1.extend(3,0); 00410 xpkmas1.set(nBotsx3+numm,0,matrix(lpos)); 00411 00412 Ematrix J1 = jacobianJ1(xrpkmas1,lpos); 00413 00414 Ematrix R = J1*Ppkmas1.subMat(r3,r3mas2, 0, nBotsx3+numm-1); 00415 Ematrix Pr = Ppkmas1.subMat(r3,r3mas2,r3,r3mas2); 00416 Ematrix Pl = J1*Pr*J1.transpose() + Pl2; 00417 00418 Ppkmas1.extend(3,3); 00419 Ppkmas1.set(0,nBotsx3+numm,R.transpose()); 00420 Ppkmas1.set(nBotsx3+numm,0,R); 00421 Ppkmas1.set(nBotsx3+numm,nBotsx3+numm,Pl); 00422 00423 H.extend(0,3); 00424 } 00425 if (mark) {delete mark; mark=0;} 00426 } // for each mark 00427 //} 00428 } 00429 }// for each robot 00430 00431 // #ifndef WIN32 00432 // gettimeofday(&tv2, NULL); 00433 // time2 = tv2.tv_sec*1000.0 + tv2.tv_usec/1000.0; 00434 // //printf("[EKF] Data Association Time: %f \n", time2-time1); 00435 // #endif 00436 00437 00438 //Ppkmas1.print("Ppkmas1"); 00439 00440 // if(markid>2){ 00441 if(markid){ 00442 Ematrix Htrans = H.transpose(); 00443 Ematrix In = (ZT - zgorro); // innovacion 00444 Ematrix S = ((H*(Ppkmas1.mul2(Htrans))) + Rt); // covarianza de la innovacion 00445 Ematrix K = (Ppkmas1.mul2((Htrans)*(S.inverse()))); // ganancia 00446 xpkmas1 = (K*In)+xpkmas1; // actualizamos el estado 00447 Ppkmas1 = Ppkmas1-K.mul2(H*Ppkmas1); // actualizamos covarianza 00448 //K.print("K="); 00449 //H.print("H="); 00450 //Rt.print("Rt="); 00451 //In.print("IN="); 00452 //ZT.print("ZT="); 00453 //zgorro.print("zgorro="); 00454 //float totalin = 3.0f*In.totalsum()/In.getNumRows(); 00455 //printf("totalin = %f\n", totalin); 00456 //if (totalin > 2 ) errorcount++; 00457 //else errorcount = 0; 00458 //if (errorcount >= 3 ) exit(-1); 00459 } 00460 //xpkmas1.subMat(0,2,0,0).toPose().print("Xk|k = "); 00461 00462 // #ifndef WIN32 00463 // gettimeofday(&tv2, NULL); 00464 // time3 = tv2.tv_sec*1000.0 + tv2.tv_usec/1000.0; 00465 // //printf("[EKF] Update Time: %f \n", time3-time2); 00466 // #endif 00467 00468 //state = xpkmas1; 00469 //covariance = Ppkmas1; 00470 00471 //xpkmas1.print("X="); 00472 //Ppkmas1.print("Cov="); 00473 00474 float uncertainty = evalUncertainty(); 00475 //printf("[EKF] uncertainty = %f\n",uncertainty); 00476 00477 // pasar los valores de las matrices al mapa 00478 for (int l = 0; l < vmap->getNLandmarks(); l++){ 00479 landmark* mark = vmap->getLandmarkById(l); 00480 if (mark){ 00481 mark->pos = state.subMat(nBotsx3+l*3,nBotsx3+l*3+2,0,0).toPos3d(); 00482 mark->covariance = covariance.subMat(nBotsx3+l*3,nBotsx3+l*3+2,nBotsx3+l*3,nBotsx3+l*3+2); 00483 vmap->changeLastReturnedLandmark(*mark); 00484 delete mark; 00485 mark = 0; 00486 } 00487 } 00488 00489 //printf("[EKF] marcas en el mapa= %d\n",vmap->getNLandmarks()); 00490 00491 //printf("\t\t\tupdate\n"); 00492 for (r=0; r<nBots; r++){ 00493 if (robotsEnabled[r]){ 00494 if (xpkmas1.get(3*r+2,3*r+2)>PI) xpkmas1.set(3*r+2,3*r+2,xpkmas1.get(3*r+2,3*r+2)- PIx2); 00495 else if (xpkmas1.get(3*r+2,3*r+2) <= -PI) xpkmas1.set(3*r+2,3*r+2,xpkmas1.get(3*r+2,3*r+2)+ PIx2); 00496 evalDisp(r); 00497 pose pos = getPos(r); 00498 robotcells[r].x = (int) floor((pos.x - xorigin)/resolution); 00499 robotcells[r].y = (int) floor((pos.y - yorigin)/resolution); 00500 00501 omap->update(*(rsData[r]), pos, getDisp(r)); // updating occupation map 00502 updatePPMap(robotcells[r], *ppmap, getDisp(r), badlocalized[r]); 00503 updateIPMap(robotcells[r], *ipmap, getDisp(r), badlocalized[r]); 00504 } 00505 } 00506 step(); 00507 00508 00509 emit slamUpdated(); 00510 00511 //if (paso%10==0){ 00512 // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> show maps >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 00513 // IplImage* im, *im2, *im3; 00514 // if (displayomap){ 00515 // //printf("[EKF] displaying maps...\n"); 00516 // im = omap->getMapAsImage(); 00517 // CvScalar redcolor = CV_RGB(255,0,0); 00518 // //int hinv = gmheight-1; 00519 // //binMap front; 00520 // //omap->frontiers(front); 00521 // //front.showMap("frontiers"); 00522 // if (displayposes) // display all particles 00523 // for (r=0; r<nBots; r++) 00524 // drawPose (*im, getPos(r), getCovariance(r), xorigin, yorigin, resolution); 00525 // if (displayfeatures) vmap->drawMarks(*im,xorigin,yorigin,resolution); 00526 // if (windowName) cvShowImage(windowName,im); 00527 // else cvShowImage("OMAP",im); 00528 00529 // //printf("voy a hacer el frame grabe %d, %d\n",saveAviFile,avifile); 00530 // if(saveAviFile && avifile){ 00531 // int res = cvWriteFrame(avifile, im); 00532 // //printf("[EKF] frame grabber, result %d\n",res); 00533 // } 00534 // //printf("[EKF] acabo\n"); 00535 // } 00536 // if (displayppmap){ 00537 // im2 = ppmap->getMapAsImage(); 00538 // cvShowImage("PPMAP",im2); 00539 // } 00540 // if (displayipmap){ 00541 // im3 = ipmap->getMapAsImage(); 00542 // cvShowImage("IPMAP",im3); 00543 // } 00544 // //printf("[EKF] aqui\n"); 00545 // //if (displayomap || displayppmap || displayipmap) cvWaitKey(2); 00546 // //printf("[EKF] aqui 2\n"); 00547 // if (displayomap) cvReleaseImage(&im); 00548 // if (displayppmap) cvReleaseImage(&im2); 00549 // if (displayipmap) cvReleaseImage(&im3); 00550 //} 00551 // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> end show maps 00552 00553 //printf("[EKF] logging...\n"); 00554 00555 // write logs 00556 if (logstr){ 00557 fprintf(slamlog,"%d",getStep()); 00558 for (r=0; r<nBots;r++){ 00559 pose p = state.subMat(r*3,r*3+2,0,0).toPose(); 00560 //fprintf(slamlog,", %f, %f, %f, %f, %f, %f, %f", p.x, p.y, p.th, getDisp(r),uncertainty,mediana,determinante); 00561 fprintf(slamlog,", %f, %f, %f, %f", p.x, p.y, p.th, getDisp(r)); 00562 } 00563 fprintf(slamlog,"\n"); 00564 } 00565 00566 // #ifndef WIN32 00567 // gettimeofday(&tv2, NULL); 00568 // curtime = tv2.tv_sec*1000.0 + tv2.tv_usec/1000.0; 00569 // //printf("[EKF] Save and show Time: %f \n", curtime-time3); 00570 // //printf("[EKF] Total Time: %f \n", curtime-initime); 00571 // #endif 00572 00573 }// while (!endSlam) 00574 00575 if (logstr){ 00576 fprintf(slamlog,"];"); 00577 fclose(slamlog); 00578 } 00579 00580 //if (saveAviFile && avifile) cvReleaseVideoWriter( &avifile ); 00581 00582 // if (displayomap){ 00583 // if (windowName) cvDestroyWindow(windowName); 00584 // else cvDestroyWindow("OMAP"); 00585 // } 00586 // if (displayppmap) cvDestroyWindow("PPMAP"); 00587 // if (displayipmap) cvDestroyWindow("IPMAP"); 00588 00589 //printf("[EKF] Stopping SLAM\n"); 00590 closing.unlock(); 00591 } 00592 00593 QImage* EKFilter::getQImage(){ 00594 // IplImage* img = omap->getMapAsImage(); 00595 // QImage* qim = new QImage((const uchar *)img->imageData, img->width, img->height, img->widthStep, QImage::Format_RGB888); 00596 // QImage* qim = slam->getQImage(); 00597 // QPixmap mapfig = QPixmap::fromImage(*qim); 00598 // cvReleaseImage(&img); 00599 // return qim; 00600 return 0; 00601 } 00602 00603 00604 QPixmap* EKFilter::getPixmap(){ 00605 // IplImage* img = omap->getMapAsImage(); 00606 // cvSaveImage("imgoctmp.jpg",img); 00607 // cvReleaseImage(&img); 00608 // QPixmap* pix = new QPixmap("imgoctmp.jpg"); 00609 // return pix; 00610 00611 IplImage* img = omap->getMapAsImage(); 00612 for (int r=0; r<nBots; r++) 00613 drawPose (*img, getPos(r), getCovariance(r), xorigin, yorigin, resolution); 00614 vmap->drawMarks(*img,xorigin,yorigin,resolution); 00615 00616 QImage* qim = new QImage((const uchar *)img->imageData, img->width, img->height, img->widthStep, QImage::Format_RGB888); 00617 QPixmap* mapfig = new QPixmap(QPixmap::fromImage(*qim)); 00618 cvReleaseImage(&img); 00619 delete qim; 00620 return mapfig; 00621 00622 } 00623 00624 00625 // odometry noise matrix Q 00626 Ematrix EKFilter::noiseQ(const pose& pos, const pose& deltaOdo, float thetaodo){ 00627 // float deltatrans2 = ((deltaOdo.x*deltaOdo.x+deltaOdo.y*deltaOdo.y)); 00628 // float dtheta = fabs(deltaOdo.th); 00629 // Ematrix Q(3,3); 00630 // Q.set(0,0, pow(alfa3*deltatrans+alfa4*dtheta,2)); 00631 // Q.set(1,1, pow(alfa2*deltatrans+alfa1*dtheta,2)); 00632 // Q.set(2,2, pow(alfa2*deltatrans+alfa1*dtheta,2)); 00633 // Ematrix Q(2,2); 00634 // Q.set(0,0, alfa3*alfa3*deltatrans2); 00635 // Q.set(0,1, alfa4*alfa4*deltaOdo.th*deltaOdo.th); 00636 // Q.set(1,0, alfa2*alfa2*deltatrans2); 00637 // Q.set(1,1, alfa1*alfa1*deltaOdo.th*deltaOdo.th); 00638 // Q.set(0,0, alfa3*alfa3*deltatrans2+alfa4*alfa4*deltaOdo.th*deltaOdo.th); 00639 // Q.set(1,1, alfa2*alfa2*deltatrans2+alfa1*alfa1*deltaOdo.th*deltaOdo.th); 00640 float deltat = sqrt((deltaOdo.x*deltaOdo.x+deltaOdo.y*deltaOdo.y)); 00641 float deltar1; 00642 if (!(fabs(deltaOdo.x) < 0.000001 && fabs(deltaOdo.y) < 0.000001)) 00643 deltar1 = atan2(deltaOdo.y,deltaOdo.x)-thetaodo; 00644 else 00645 deltar1 = 0; 00646 float deltar2 = deltaOdo.th - deltar1; 00647 if (deltar1>PI) deltar1-= PIx2; 00648 else if (deltar1 <= -PI) deltar1+= PIx2; 00649 if (deltar2>PI) deltar2-= PIx2; 00650 else if (deltar2 <= -PI) deltar2+= PIx2; 00651 Ematrix Q(3,3); 00652 Q.set(0,0, pow(alfa1*fabs(deltar1) + alfa2*deltat ,2)); 00653 Q.set(1,1, pow(alfa3*deltat + alfa4*(fabs(deltar1+deltar2)) ,2)); 00654 Q.set(2,2, pow(alfa1*fabs(deltar2) + alfa2*deltat ,2)); 00655 return Q; 00656 } 00657 00658 // Motion model 00659 matrix EKFilter::motionModel (const pose& pos, const pose& deltaOdo, float thetaodo){ 00660 float deltatrans = sqrt((deltaOdo.x*deltaOdo.x+deltaOdo.y*deltaOdo.y)); 00661 float aux = atan2(deltaOdo.y,deltaOdo.x) - thetaodo + pos.th; 00662 if (aux>PI) aux-= PIx2; 00663 else if (aux <= -PI) aux+= PIx2; 00664 // float aux = deltaOdo.th/2+pos.th; /// OJO ESTO PODRIA ESTAR MAL!!!! pag 134 prob robotics eqs 2 3 4 00665 // printf("aux1 = %15.15f, aux2 = %15.15f ", aux, aux2); 00666 // if (fabs(aux - aux2) > 0.000000001) printf(" >>>>>>>>>>> SON DISTINTOS!!!!!!\n"); 00667 // else printf("ok\n"); 00668 matrix X(3,1); 00669 X.set(0,0,deltatrans*cos(aux)+pos.x); 00670 X.set(1,0,deltatrans*sin(aux)+pos.y); 00671 X.set(2,0,pos.th+deltaOdo.th); 00672 if (X.get(2,0)>PI) X.set(2,0,X.get(2,0)- PIx2); 00673 else if (X.get(2,0) <= -PI) X.set(2,0,X.get(2,0)+ PIx2); 00674 return X; 00675 } 00676 00677 // jacobian F 00678 matrix EKFilter::jacobianF(const pose& pos, const pose& deltaOdo, float thetaodo){ 00679 float deltatrans = sqrt((deltaOdo.x*deltaOdo.x+deltaOdo.y*deltaOdo.y)); 00680 float aux = atan2(deltaOdo.y,deltaOdo.x) - thetaodo + pos.th; 00681 if (aux>PI) aux-= PIx2; 00682 else if (aux <= -PI) aux+= PIx2; 00683 // float aux = deltaOdo.th/2+pos.th; /// OJO ESTO PODRIA ESTAR MAL!!!! pag 134 prob robotics eqs 2 3 4 00684 matrix F(3,3); 00685 F.set(0,0,1); 00686 F.set(0,2,-deltatrans*sin(aux)); 00687 F.set(1,1,1); 00688 F.set(1,2, deltatrans*cos(aux)); 00689 F.set(2,2,1); 00690 return F; 00691 } 00692 00693 // jacobian G 00694 Ematrix EKFilter::jacobianG(const pose& pos, const pose& deltaOdo, float thetaodo){ 00695 float deltatransm = sqrt((deltaOdo.x*deltaOdo.x+deltaOdo.y*deltaOdo.y)); 00696 float aux = atan2(deltaOdo.y,deltaOdo.x) - thetaodo + pos.th; 00697 if (aux>PI) aux-= PIx2; 00698 else if (aux <= -PI) aux+= PIx2; 00699 // float aux = deltaOdo.th/2+pos.th; /// OJO ESTO PODRIA ESTAR MAL!!!! pag 134 prob robotics eqs 2 3 4 00700 Ematrix G(3,3,3+3*nmarks,3); 00701 // Ematrix G(3,2,3+3*nmarks,2); 00702 G.set(0,0,-deltatransm*sin(aux)); 00703 G.set(0,1, cos(aux)); 00704 G.set(1,0, deltatransm*cos(aux)); 00705 G.set(1,1, sin(aux)); 00706 G.set(2,0,1); 00707 G.set(2,2,1); 00708 return G; 00709 } 00710 00711 // jacobian H 00712 matrix EKFilter::jacobianH1(const pose& pos, const pos3d& mark){ 00713 matrix H(3,3); 00714 float costh = (float)cos(pos.th); 00715 float sinth = (float)sin(pos.th); 00716 float auxx = mark.getX() - pos.x; 00717 float auxy = mark.getY() - pos.y; 00718 H.set(0,0,-costh); 00719 H.set(0,1,-sinth); 00720 H.set(0,2,-(auxx)*sinth + (auxy)*costh ); 00721 H.set(1,0, sinth); 00722 H.set(1,1,-costh); 00723 H.set(1,2,-(auxx)*costh - (auxy)*sinth ); 00724 return H; 00725 } 00726 00727 matrix EKFilter::jacobianH2(const pose& pos){ 00728 matrix H(3,3); 00729 float costh = (float)cos(pos.th); 00730 float sinth = (float)sin(pos.th); 00731 H.set(0,0,costh); 00732 H.set(0,1,sinth); 00733 H.set(1,0,-sinth); 00734 H.set(1,1,costh); 00735 H.set(2,2,1.0f); 00736 return H; 00737 } 00738 00739 // jacobian J1 00740 matrix EKFilter::jacobianJ1(const pose& pos, const pos3d& mark){ 00741 matrix J1(3,3); 00742 float costh = (float)cos(pos.th); 00743 float sinth = (float)sin(pos.th); 00744 pos3d p = vmap->cam2base(mark); 00745 J1.set(0,0,1); 00746 J1.set(0,2,-p.getX()*sinth - p.getY()*costh); 00747 J1.set(1,1,1); 00748 J1.set(1,2, p.getX()*costh - p.getY()*sinth); 00749 J1.set(2,2,1); 00750 return J1; 00751 } 00752 00753 // asociacion de datos 00754 landmark* EKFilter::dataAssociation(const matrix& ZT, const matrix& Rt, const pos3d& globalpos, const pose& robotPos, const matrix& H2, const matrix& H2trans, const float* desc, const Ematrix& Pcomp, int r) { 00755 float minDist=descTh, dist; 00756 float maxDist=mahTh; 00757 //landmark* res = 0; 00758 landmark* tmpmark = 0; 00759 int markid=-1; 00760 // printf("Comienzo asociacion de datos>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"); 00761 for (int i = 0; i< vmap->getNLandmarks(); i++){ 00762 tmpmark = vmap->getLandmarkById(i); 00763 // if (sqrt( pow(tmpmark->pos.getX()-globalpos.getX(),2)+ pow(tmpmark->pos.getY()-globalpos.getY(),2))<range){ 00764 00765 matrix H(3,6); 00766 H.set(0,0,jacobianH1(robotPos,tmpmark->pos)); 00767 H.set(0,3,H2); 00768 matrix Htrans = H.transpose(); 00769 matrix P(6,6); 00770 00771 P.set(0,0,Pcomp.subMat(r*3,r*3+3,r*3,r*3+3)); 00772 P.set(0,3,Pcomp.subMat(r*3,r*3+3,3*nBots+i*3,3*nBots+i*3+3)); 00773 P.set(3,0,Pcomp.subMat(3*nBots+i*3,3*nBots+i*3+3,r*3,r*3+3)); 00774 P.set(3,3,Pcomp.subMat(3*nBots+i*3,3*nBots+i*3+3,3*nBots+i*3,3*nBots+i*3+3)); 00775 00776 matrix zgorro = vmap->base2cam(global2base(tmpmark->pos, robotPos)); 00777 matrix In = ZT - zgorro; 00778 matrix Z = (H*(P*(Htrans))) + Rt; ///////////////////////////////////////////////////////////// 00779 00780 //float pmatch = exp(-0.5*vmap->mahalanobis(In.toPos3d(), Z)) /(sqrt((Z*PIx2).det())); 00781 float pmatch = vmap->mahalanobis(In.toPos3d(), Z); 00782 if (tmpmark->descriptor[0]==desc[0]){ 00783 if (pmatch > maxCorrect){ 00784 maxCorrect = pmatch; 00785 /* printf("Desc %f\n",desc[0]); 00786 printf("match = %f\n",pmatch); 00787 globalpos.print("new estimated pos="); 00788 tmpmark->pos.print("map position="); 00789 robotPos.print("robot pose="); 00790 ZT.print("ZT="); 00791 zgorro.print("zgorro="); 00792 P.print("P="); 00793 Rt.print("Rt="); 00794 H.print("H="); 00795 In.print("IN="); 00796 Z.print("Z="); 00797 */ } 00798 } 00799 else{ 00800 if (pmatch < minIncorrect){ 00801 minIncorrect = pmatch; 00802 /* printf("Desc %f\n",desc[0]); 00803 globalpos.print("new estimated pos="); 00804 tmpmark->pos.print("map position="); 00805 robotPos.print("robot pose="); 00806 ZT.print("ZT="); 00807 zgorro.print("zgorro="); 00808 P.print("P="); 00809 Rt.print("Rt="); 00810 H.print("H="); 00811 In.print("IN="); 00812 Z.print("Z="); 00813 */ } 00814 } 00815 //printf("maxCorrect=%f, minIncorrect = %f\n",maxCorrect,minIncorrect); 00816 if ( pmatch < mahTh){ // check mahalanobis 00817 dist = visualMap::descDist(tmpmark->descriptor, desc, tmpmark->desclength); 00818 if (dist < descTh){ // get the minor descriptor distance but bigger than descTh 00819 if (nearestNeighbourOrderBy == 0){ // nearest desc 00820 if(dist < minDist){ 00821 markid = i; 00822 minDist = dist; 00823 } 00824 } 00825 else{ // nearest mahalanobis 00826 if(pmatch < maxDist){ 00827 markid = i; 00828 maxDist = pmatch; 00829 } 00830 } 00831 } 00832 } 00833 //} 00834 delete tmpmark; tmpmark=0; 00835 } 00836 //printf("fin de la asociacion de datos\n"); 00837 landmark* match = 0; 00838 if (markid>=0){ 00839 match = vmap->getLandmarkById(markid); 00840 //if (desc[0]!= match->descriptor[0]) printf("DATA ASSOCIATION ERRORRR!!!!!!!!!!!\n"); 00841 } 00842 //printf("Fin asociacion de datos>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"); 00843 00844 return (markid>=0)? match:0; 00845 } 00846 00847 00848 // evaluates the uncertainty on the position of each robot 00849 void EKFilter::evalDisp(int r){ 00850 matrix covariance = getCovariance(r); 00851 00852 CvMat* cov = cvCreateMat(2,2,CV_32FC1); 00853 CvMat* evects = cvCreateMat(2,2,CV_32FC1); 00854 CvMat* evals = cvCreateMat(2,1,CV_32FC1); 00855 cvmSet(cov,0,0, covariance(0,0)); 00856 cvmSet(cov,0,1, covariance(0,1)); 00857 cvmSet(cov,1,0, covariance(1,0)); 00858 cvmSet(cov,1,1, covariance(1,1)); 00859 00860 cvEigenVV( cov, evects, evals, DBL_EPSILON); 00861 00862 float a = 3*sqrt(cvmGet(evals,0,0)); 00863 float b = 3*sqrt(cvmGet(evals,1,0)); 00864 if (a != a) a = 0.0f; 00865 if (b != b) b = 0.0f; 00866 00867 cvReleaseMat(&cov); 00868 cvReleaseMat(&evects); 00869 cvReleaseMat(&evals); 00870 00871 dispRobot[r] = PI*a*b; 00872 00873 if (badlocalized[r] == false && dispRobot[r]>th_high) badlocalized[r] = true; 00874 if (badlocalized[r] == true && dispRobot[r]<th_low) badlocalized[r] = false; 00875 } 00876 00877 float EKFilter::evalUncertainty(){ 00878 /* 00879 CvMat* cov = cvCreateMat(covariance.getNumRows(),covariance.getNumCols(),CV_32FC1); 00880 CvMat* evects = cvCreateMat(covariance.getNumRows(),covariance.getNumCols(),CV_32FC1); 00881 CvMat* evals = cvCreateMat(1,covariance.getNumCols(),CV_32FC1); 00882 for (int i = 0 ; i < covariance.getNumRows(); i++) 00883 for (int j = 0 ; j < covariance.getNumCols(); j++) 00884 cvmSet(cov,i,j, covariance(i,j)); 00885 00886 cvEigenVV( cov, evects, evals, DBL_EPSILON); 00887 */ 00888 float trace = 0; 00889 //list<float> eigenvalues; 00890 //determinante = 0.0; 00891 for (int i = 0; i < covariance.getNumRows(); i++){ 00892 trace += covariance(i,i); 00893 //determinante += log(cvmGet(evals,0,i)); 00894 //eigenvalues.push_back(cvmGet(evals,0,i)); 00895 } 00896 trace /= covariance.getNumRows(); 00897 //determinante -= log((float)covariance.getNumRows()); 00898 00899 //eigenvalues.sort(); 00900 //mediana = 0.0f; 00901 //for (unsigned int v = 0; v < eigenvalues.size()/2; v++) eigenvalues.pop_back(); 00902 //mediana = eigenvalues.back(); 00903 00904 //cvReleaseMat(&cov); 00905 //cvReleaseMat(&evects); 00906 //cvReleaseMat(&evals); 00907 00908 return trace; 00909 } 00910 00911 00912 00913 // maps 00914 gridMapInterface* EKFilter::getOMap() { 00915 return omap->clone(); 00916 } 00917 00918 visualMap* EKFilter::getVMap() { 00919 return vmap->clone(); 00920 } 00921 00922 binMap* EKFilter::getPreMap() { 00923 return new binMap(*ppmap); 00924 } 00925 00926 binMap* EKFilter::getImpMap() { 00927 return new binMap(*ipmap); 00928 } 00929 00930 /// returns the matrix that represents the covariance of robots and marks 00931 Ematrix EKFilter::getGlobalCovariance() const{ 00932 return(covariance); 00933 }; 00934 00935 /// returns the current pose of the robot 00936 pose EKFilter::getPos(int r){ 00937 return state.subMat(r*3,r*3+2,0,0).toPose(); 00938 } 00939 00940 /// returns the current cell of the robot 00941 point EKFilter::getCell(int robot){ 00942 return robotcells[robot]; 00943 } 00944 00945 /// returns the matrix that represents the covariance of the position of the robot 00946 matrix EKFilter::getCovariance(int r) const{ 00947 return covariance.subMat(r*3,r*3+2,r*3,r*3+2); 00948 } 00949 00950 /// returns a measure of the dispersion of the robot 00951 float EKFilter::getDisp(int r) const{ 00952 return dispRobot[r]; 00953 } 00954 00955 void EKFilter::drawPose (IplImage& im, const pose& pos, const matrix& covariance, float xorigin, float yorigin, float resolution) { 00956 00957 //pos.print("pos="); 00958 //covariance.print("Cov="); 00959 //printf("%e %e %e\n",covariance.get(0,0),covariance.get(1,1),covariance.get(2,2)); 00960 00961 CvScalar redcolor = CV_RGB(255,0,0); 00962 CvPoint p = cvPoint ( (int) floor ( (pos.x - xorigin)/resolution), // posicion 00963 (int) (im.height-1-floor((pos.y - yorigin)/resolution))); 00964 CvPoint p2 = cvPoint ( (int) floor ( (pos.x+0.5*cos(pos.th) - xorigin)/resolution), // posicion 00965 (int) (im.height-1-floor((pos.y+0.5*sin(pos.th) - yorigin)/resolution))); 00966 00967 CvMat* cov = cvCreateMat(2,2,CV_32FC1); 00968 CvMat* evects = cvCreateMat(2,2,CV_32FC1); 00969 CvMat* evals = cvCreateMat(2,1,CV_32FC1); 00970 cvmSet(cov,0,0, covariance(0,0)); 00971 cvmSet(cov,0,1, covariance(0,1)); 00972 cvmSet(cov,1,0, covariance(1,0)); 00973 cvmSet(cov,1,1, covariance(1,1)); 00974 00975 cvEigenVV( cov, evects, evals, DBL_EPSILON); 00976 00977 float a = 3*sqrt(cvmGet(evals,0,0)); 00978 float b = 3*sqrt(cvmGet(evals,1,0)); 00979 float alfa; 00980 00981 if (a != a) a = 0.0f; 00982 if (b != b) b = 0.0f; 00983 if (a<b){ 00984 float tmp = a; 00985 a = b; 00986 b = tmp; 00987 alfa = 180.0f/PI*atan2(cvmGet(evects,0,0),cvmGet(evects,0,1)); 00988 } 00989 else alfa = 180.0f/PI*atan2(cvmGet(evects,0,1),cvmGet(evects,0,0)); 00990 00991 //printf("a= %f, b=%f\n",a,b); 00992 00993 int ia = (int)floor(a/resolution+0.5); 00994 int ib = (int)floor(b/resolution+0.5); 00995 00996 if (ia >= 0 && ia < im.width && ib >= 0 && ib < im.width){ 00997 cvEllipse(&im, p, cvSize(ia,ib), alfa, 0, 360, redcolor, CV_FILLED ); 00998 } 00999 01000 // cvLine(&im, p, p2, cvScalar(255,0,0), 1); 01001 01002 01003 cvReleaseMat(&cov); 01004 cvReleaseMat(&evects); 01005 cvReleaseMat(&evals); 01006 01007 }
1.7.6.1
