|
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 /* 00002 * 00003 * Author: Miguel Julia <mjulia@umh.es> 00004 * 00005 * Date: 2008 00006 * 00007 * Class RBPFilter 00008 * 00009 * Implements an slam algorithm consisting on a rao-blackwellized particle filter 00010 * 00011 */ 00012 00013 #include "RBPFilter.h" 00014 #include "gridMapInterface.h" 00015 #include <math.h> 00016 #include <iostream> 00017 #include <stdlib.h> 00018 #include <opencv2/opencv.hpp> 00019 00020 #ifndef WIN32 00021 #include <sys/time.h> 00022 #else 00023 #include <process.h> 00024 #endif 00025 00026 using namespace std; 00027 00028 // Para registrar la clase en la factoria 00029 namespace{ 00030 slamInterface* CreateRBPFilter(int bots, ConfigFile& file){ 00031 return new RBPFilter(bots, file); 00032 } 00033 const int RBPF = 0; 00034 const bool registered = slamFactory::Instance().Register(RBPF, slamCreator(CreateRBPFilter)); 00035 } 00036 00037 // Constructor 00038 RBPFilter::RBPFilter(): 00039 vslamFilter(), 00040 M(0), 00041 p(0), 00042 newp(0), 00043 index(0), 00044 dispRobot(0), 00045 odoData(0), 00046 lastOdoData(0), 00047 rsData(0), 00048 lmData(0), 00049 endSlam(false), 00050 onlyonegridmap(true), 00051 omap(0), 00052 ppmap(0), 00053 ipmap(0), 00054 Identity3(matrix::identity(3)), 00055 mahTh(0.0f) 00056 { 00057 // printf("RBPF default constructor\n"); 00058 } 00059 00060 RBPFilter::RBPFilter(int nrobots, ConfigFile& configFile): 00061 vslamFilter (nrobots,configFile), 00062 M (nrobots*configFile.read<int>("PARTICLESPERROBOT")), 00063 p (new particle[M]()), 00064 newp (new particle[M]()), 00065 index (0), 00066 dispRobot (new float[nBots]), 00067 odoData (new pose*[nBots]), 00068 lastOdoData (new pose*[nBots]), 00069 rsData (new rangeSensorData*[nBots]), 00070 lmData (new landmarksData*[nBots]), 00071 endSlam (true), 00072 onlyonegridmap (configFile.keyExists("ONLYONEGRIDMAP")? configFile.read<bool>("ONLYONEGRIDMAP") : true ), 00073 omap (0), 00074 ppmap (0), 00075 ipmap (0), 00076 Identity3 (matrix::identity(3)), 00077 mahTh (configFile.read<float>("MAHALANOBISTH")) 00078 { 00079 if (onlyonegridmap){ 00080 omap = gridMapFactory::Instance().CreateObject(gmtype,width, height, resolution, xorigin, yorigin); 00081 ppmap = new binMap(gmwidth, gmheight, resolution, xorigin, yorigin); 00082 ipmap = new binMap(gmwidth, gmheight, resolution, xorigin, yorigin); 00083 } 00084 int gt = (onlyonegridmap)? -1: gmtype; 00085 for (int m = 0; m< M; m++) 00086 p[m].initialize(nrobots, width, height, resolution, xorigin, yorigin, gt, vmtype, nmarks, configFile); 00087 for (int r = 0; r< nBots; r++){ 00088 dispRobot[r] = 0.0f; 00089 } 00090 // printf("RBPF created\n"); 00091 } 00092 00093 RBPFilter::~RBPFilter(){ 00094 // printf("RBPF destroyer..."); 00095 stop(); 00096 if (logstr){ 00097 char myfilestr[100]; 00098 if (onlyonegridmap){ 00099 sprintf(myfilestr,"%somap.jpg",logstr); 00100 omap->saveMapAsImage(myfilestr); 00101 sprintf(myfilestr,"%sppmap.jpg",logstr); 00102 ppmap->saveMapAsImage(myfilestr); 00103 } 00104 else{ 00105 sprintf(myfilestr,"%somap.jpg",logstr); 00106 p[index].getOMap().saveMapAsImage(myfilestr); 00107 sprintf(myfilestr,"%sppmap.jpg",logstr); 00108 p[index].getPrecisePoseMap().saveMapAsImage(myfilestr); 00109 } 00110 sprintf(myfilestr,"%svmap",logstr); 00111 p[index].getVMap().saveMap(myfilestr); 00112 } 00113 if (p) delete[] p; 00114 if (newp) delete[] newp; 00115 if (dispRobot) delete[] dispRobot; 00116 if (odoData) delete[] odoData; 00117 if (lastOdoData) delete[] lastOdoData; 00118 if (rsData) delete[] rsData; 00119 if (lmData) delete[] lmData; 00120 if (omap) delete omap; 00121 if (ppmap) delete ppmap; 00122 if (ipmap) delete ipmap; 00123 00124 // printf("RBPF destroyed\n"); 00125 } 00126 00127 // Initializer 00128 void RBPFilter::initialize(int nrobots, ConfigFile& configFile){ 00129 vslamFilter::initialize(nrobots, configFile); 00130 00131 M = configFile.read<int>("PARTICLESPERROBOT")*nBots; 00132 if (p) delete[] p; 00133 p = new particle[M](); 00134 if (newp) delete[] newp; 00135 newp = new particle[M](); 00136 index = 0; 00137 if (dispRobot) delete[] dispRobot; 00138 dispRobot = new float[nBots]; 00139 if (odoData) delete[] odoData; 00140 odoData = new pose*[nBots]; 00141 if (lastOdoData) delete[] lastOdoData; 00142 lastOdoData = new pose*[nBots]; 00143 if (rsData) delete[] rsData; 00144 rsData = new rangeSensorData*[nBots]; 00145 if (lmData) delete[] lmData; 00146 lmData = new landmarksData*[nBots]; 00147 endSlam = false; 00148 onlyonegridmap = configFile.keyExists("ONLYONEGRIDMAP")? configFile.read<bool>("ONLYONEGRIDMAP") : true ; 00149 mahTh = configFile.read<float>("MAHALANOBISTH"); 00150 00151 if (onlyonegridmap){ 00152 if (omap) delete omap; 00153 if (ppmap) delete ppmap; 00154 if (ipmap) delete ipmap; 00155 omap = gridMapFactory::Instance().CreateObject(gmtype, width, height, resolution, xorigin, yorigin); 00156 ppmap = new binMap(gmwidth, gmheight, resolution, xorigin, yorigin); 00157 ipmap = new binMap(gmwidth, gmheight, resolution, xorigin, yorigin); 00158 } 00159 00160 int gt = (onlyonegridmap)? -1: gmtype; 00161 for (int m = 0; m< M; m++) 00162 p[m].initialize(nrobots, width, height, resolution, xorigin, yorigin, gt, vmtype, nmarks, configFile); 00163 for (int r = 0; r< nBots; r++){ 00164 dispRobot[r] = 0.0f; 00165 } 00166 } 00167 00168 int RBPFilter::setup(){ 00169 prio = 50; 00170 return 0; 00171 } 00172 00173 void RBPFilter::onStop(){ 00174 if (!endSlam){ 00175 endSlam = true; 00176 closing.lock(); 00177 closing.unlock(); 00178 } 00179 printf("[FastSLAM] SLAM stopped\n"); 00180 } 00181 00182 void RBPFilter::execute(){ 00183 printf("[FastSLAM] \t\t\t\t\t\t Running SLAM\n"); 00184 closing.lock(); 00185 int r; 00186 00187 // open the log file 00188 FILE* slamlog=0; 00189 if (logstr){ 00190 char myfilestr[100]; 00191 sprintf(myfilestr,"%sslam.m",logstr); 00192 slamlog = fopen(myfilestr,"w"); 00193 fprintf(slamlog,"poses_est=["); 00194 } 00195 00196 if (displayomap) cvNamedWindow("OMAP", CV_WINDOW_AUTOSIZE); 00197 if (displayppmap) cvNamedWindow("PPMAP", CV_WINDOW_AUTOSIZE); 00198 if (displayipmap) cvNamedWindow("IPMAP", CV_WINDOW_AUTOSIZE); 00199 00200 // inicializamos SLAM 00201 00202 // printf("[FastSLAM 1] initializing slam...\n"); 00203 for (r=0; r<nBots; r++){ 00204 if (rbase[r]) rbase[r]->beginConsumition(); 00205 lastOdoData[r] = rbase[r]->getOdometry(); 00206 //printf("initial pose r%d: x:%f, y%f, th=%f\n",r,lastOdoData[r]->x,lastOdoData[r]->y,lastOdoData[r]->th); 00207 for (int i = 0; i < M; i++){ 00208 p[i].setPos(*(lastOdoData[r]),r); 00209 } 00210 if (rbase[r]) rbase[r]->endConsumition(); 00211 } 00212 // printf("[FastSLAM 1] slam initialized\n"); 00213 00214 float cw = 1.0f/M; 00215 for (int i = 0; i < M; i++){ 00216 p[i].setWeight(cw); // Resetamos el peso de las particulas 00217 } 00218 00219 endSlam=false; 00220 while (!endSlam){ 00221 //printID("SLAM RBPF"); 00222 //Sleep(10); 00223 // Leemos sensores 00224 00225 // printf("[FastSLAM 1] slam step\n"); 00226 for (r=0; r<nBots; r++){ 00227 if (robotsEnabled[r]){ 00228 rbase[r]->beginConsumition(); 00229 if (robotsEnabled[r]){ 00230 odoData[r] = rbase[r]->getOdometry(); 00231 rsData[r] = rbase[r]->getRangeSensorData(); 00232 lmData[r] = rbase[r]->getLandmarksData(); 00233 } 00234 rbase[r]->endConsumition(); 00235 } 00236 } 00237 // printf("[FastSLAM 1] consumed\n"); 00238 00239 // printf("[FastSLAM 1] fastslam found: %d, in the map: %d\n",lmData[0]->getNLandmarks(), p[index].getVMap().getNLandmarks()); 00240 00241 // Fastslam 00242 for (r=0; r<nBots; r++){ 00243 if (robotsEnabled[r]){ 00244 pose deltaOdo = *(odoData[r]) - *(lastOdoData[r]); 00245 fastSlam(r, *(lmData[r]), *(lastOdoData[r]), deltaOdo); 00246 } 00247 } 00248 // printf("[FastSLAM 1] resample\n"); 00249 double neff = resample(); // resampling 00250 // printf("[FastSLAM 1] neff=%f\n",neff); 00251 //printf("estimated pose x: %f, y: %f, th: %f\n",p[index].getPos(0).x,p[index].getPos(0).y,p[index].getPos(0).th); 00252 00253 // printf("[FastSLAM 1] update\n"); 00254 for (r=0; r<nBots; r++) updateAll(*(rsData[r]),r); // updating occupation map 00255 step(); 00256 // printf("[FastSLAM 1] clean data\n"); 00257 // delete sensor data 00258 for (r=0; r<nBots; r++){ 00259 if (robotsEnabled[r]){ 00260 if (lastOdoData[r]) delete lastOdoData[r]; 00261 lastOdoData[r] = odoData[r]; 00262 if (rsData[r]) delete rsData[r]; 00263 if (lmData[r]) delete lmData[r]; 00264 } 00265 } 00266 00267 emit slamUpdated(); 00268 00269 /* // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> show maps >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 00270 printf("\t\t\tshow\n"); 00271 IplImage* im, *im2, *im3; 00272 if (displayomap){ 00273 im = (onlyonegridmap)? omap->getMapAsImage(): p[index].getOMap().getMapAsImage(); 00274 CvScalar redcolor = CV_RGB(255,0,0); 00275 int hinv = gmheight-1; 00276 if (displayposes) // display all particles 00277 for(int m=0; m<M; m++) 00278 for (r=0; r<nBots; r++) 00279 cvCircle( im, cvPoint(p[m].getCell(r).x, hinv - p[m].getCell(r).y), 0, redcolor, 1 ); 00280 if (displayfeatures) p[index].getVMap().drawMarks(*im,xorigin,yorigin,resolution); 00281 if (windowName) cvShowImage(windowName,im); 00282 else cvShowImage("OMAP",im); } 00283 if (displayppmap){ 00284 im2 = (onlyonegridmap)? ppmap->getMapAsImage(): p[index].getPrecisePoseMap().getMapAsImage(); 00285 cvShowImage("PPMAP",im2); 00286 } 00287 if (displayipmap){ 00288 im3 = (onlyonegridmap)? ipmap->getMapAsImage(): p[index].getImprecisePoseMap().getMapAsImage(); 00289 cvShowImage("IPMAP",im3); 00290 } 00291 00292 if (displayomap || displayppmap || displayipmap) cvWaitKey(10); 00293 if (displayomap) cvReleaseImage(&im); 00294 if (displayppmap) cvReleaseImage(&im2); 00295 if (displayipmap) cvReleaseImage(&im3); 00296 00297 // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> end show maps 00298 */ 00299 // write logs 00300 if (logstr){ 00301 fprintf(slamlog,"%d",getStep()); 00302 for (r=0; r<nBots;r++){ 00303 fprintf(slamlog,", %f, %f, %f, %f",p[index].getPos(r).x, p[index].getPos(r).y, p[index].getPos(r).th, getDisp(r)); 00304 } 00305 fprintf(slamlog,", %f\n", neff); 00306 } 00307 00308 // printf("[FastSLAM 1] end loop\n"); 00309 }// while (!endSlam) 00310 00311 if (logstr){ 00312 fprintf(slamlog,"];"); 00313 fclose(slamlog); 00314 } 00315 00316 if (displayomap) cvDestroyWindow("OMAP"); 00317 if (displayppmap) cvDestroyWindow("PPMAP"); 00318 if (displayipmap) cvDestroyWindow("IPMAP"); 00319 00320 // printf("[FastSLAM 1] Stopping SLAM\n"); 00321 closing.unlock(); 00322 } 00323 00324 00325 void RBPFilter::updateParticle(int robot, const landmarksData& lmData, const pose& lastOdo, const pose& deltaOdo, int i){ 00326 00327 // Hacemos avanzar la pose del robot en la particula segun la odometria y el ruido 00328 p[i].setPos(odometryModel(lastOdo, deltaOdo, p[i].getPos(robot) ),robot); 00329 00330 matrix G = jacobian(p[i].getPos(robot)); // jacobiano 00331 matrix Ginv = G.inverse(); 00332 matrix Gtrans = G.transpose(); 00333 matrix Ginvtrans = Ginv.transpose(); 00334 00335 //printf("marcas %d\n",lmData.getNLandmarks()); 00336 for (int j = 0; j < lmData.getNLandmarks(); j++){ // Se aplica de manera repetida para todas las medidas realizadas. Se multiplican los pesos 00337 00338 pos3d lpos = base2global(p[i].getVMap().cam2base(lmData.getLandmark(j).pos), p[i].getPos(robot)); // medida en coordenadas globales 00339 matrix ZT = lmData.getLandmark(j).pos; // Medida (coordenadas de camara) 00340 matrix Rt = lmData.getLandmark(j).covariance; // covarianza de la medida 00341 00342 //printf("mark medi %d, (x: %f, y: %f, z: %f)\n", j, lmData.getLandmark(j).pos.getX(), lmData.getLandmark(j).pos.getY(), lmData.getLandmark(j).pos.getZ()); 00343 //printf("mark real %d, (x: %f, y: %f, z: %f)\n", j, lpos.getX(), lpos.getY(), lpos.getZ()); 00344 00345 // associacion de datos 00346 landmark* mark = (perfectMatching)? 00347 p[i].getVMap().getLandmarkByDesc(lmData.getLandmark(j).descriptor) : 00348 p[i].getVMap().dataAssociation(ZT, Rt, lpos, p[i].getPos(robot), G, Gtrans, lmData.getLandmark(j).descriptor) ; 00349 00350 00351 // Si es una nueva landmark, la inicializamos 00352 if(!mark) { 00353 mark = new landmark(lpos, // global pos 00354 Ginv*(Rt*(Ginvtrans)), // covariance 00355 lmData.getLandmark(j).descriptor, // descriptor 00356 lmData.getLandmark(j).desclength, // descriptor length 00357 0); // repeated 00358 p[i].getVMap().addLandmark(*mark); // la insertamos en el mapa de la particula 00359 p[i].setWeight(p[i].getWeight()*mahTh); // Dividimos el peso de la particula 00360 } 00361 // Si es una landmark anterior, le calculamos su EKF 00362 else { 00363 matrix zgorro = p[i].getVMap().base2cam(global2base(mark->pos, p[i].getPos(robot))); // medida estimada 00364 matrix In = (ZT - zgorro); // innovacion 00365 matrix Z = ((G*(mark->covariance*(Gtrans))) + Rt); // covarianza de la innovacion 00366 matrix Zinv = Z.inverse(); 00367 matrix K = (mark->covariance*((Gtrans)*(Zinv))); // ganancia 00368 mark->pos = ((K*In)+mark->pos).toPos3d(); // actualizamos pose 00369 mark->covariance= (Identity3-K*G)*mark->covariance; // actualizamos covarianza 00370 00371 for (int k=0; k< mark->desclength; k++) 00372 mark->descriptor[k] = (mark->descriptor[k] + lmData.getLandmark(j).descriptor[k])/2; 00373 00374 p[i].getVMap().changeLastReturnedLandmark(*mark); 00375 00376 // Calculamos el nuevo peso 00377 p[i].setWeight(p[i].getWeight()*exp(-0.5*visualMap::mahalanobis(In.toPos3d(), Z)) /(sqrt((Z*PIx2).det()))); 00378 } 00379 delete mark; 00380 } 00381 } 00382 00383 // fastslam algorithm 00384 void RBPFilter::fastSlam(int robot, const landmarksData& lmData, const pose& lastOdo, const pose& deltaOdo){ 00385 int i; 00386 00387 //printf("\t\t\tfastslam...\n"); 00388 // update particles 00389 for (i = 0; i < M; i++){ 00390 updateParticle(robot,lmData,lastOdo,deltaOdo,i); 00391 } 00392 //printf("\t\t\tok\n"); 00393 00394 // Normalize weight 00395 double suma = 0; 00396 //double sumalog = 0; 00397 for (i = 0; i < M; i++) suma += p[i].getWeight(); 00398 if (suma > 0){ 00399 for (i = 0; i < M; i++){ 00400 p[i].setWeight(p[i].getWeight()/suma); 00401 p[i].setSumLogWeight(p[i].getSumLogWeight() + log(p[i].getWeight())); 00402 } 00403 } 00404 else{ 00405 // printf("[FastSLAM 1] Null sum of weigths!!!! - Resetting to ones\n"); 00406 double invM = 1.0f/M; 00407 for (int i = 0; i < M; i++){ 00408 p[i].setWeight(invM); 00409 p[i].setSumLogWeight(0.0); 00410 } 00411 } 00412 } 00413 00414 // update occupancy, precise poses and imprecise poses maps 00415 void RBPFilter::updateAll(const rangeSensorData& rsData, int r ){ 00416 evalDisp(r); 00417 if (onlyonegridmap){ 00418 omap->update(rsData, p[index].getPos(r), getDisp(r)); 00419 updatePPMap(p[index].getCell(r), *ppmap, getDisp(r), badlocalized[r]); 00420 updateIPMap(p[index].getCell(r), *ipmap, getDisp(r), badlocalized[r]); 00421 } 00422 else{ 00423 for (int m = 0; m < M ; m++){ 00424 p[m].getOMap().update(rsData, p[m].getPos(r), getDisp(r)); 00425 updatePPMap(p[m].getCell(r), p[m].getPrecisePoseMap(), getDisp(r), badlocalized[r]); 00426 updateIPMap(p[m].getCell(r), p[m].getImprecisePoseMap(), getDisp(r), badlocalized[r]); 00427 } 00428 } 00429 } 00430 00431 00432 // importance resampling 00433 double RBPFilter::resample(){ 00434 00435 int i,m; 00436 double suma = 0; 00437 int newindex = 0; 00438 double maxweight = p[0].getWeight(); 00439 00440 for (i = 0; i < M; i++){ 00441 suma += p[i].getWeight()*p[i].getWeight(); 00442 if(p[i].getSumLogWeight() > maxweight){ 00443 newindex = i; 00444 maxweight = p[i].getSumLogWeight(); 00445 } 00446 } 00447 double Neff = 1/suma; 00448 00449 if (Neff > M + 1 || Neff < 1){ 00450 // printf("[FastSLAM] error\n"); 00451 exit(0); 00452 } 00453 00454 if (Neff < M){ 00455 00456 // muestreo con resposicion de M muestras equiespaciadas aleatorio 00457 int* resamp = new int[M]; 00458 00459 double r = ((double(rand())+1)/(double(RAND_MAX)+2))/M; // me ha fallado alguna vez 00460 //double r = 0.5/M; // tambien se puede poner fijo a la mitad 00461 double u = r; 00462 int j = 0; 00463 double c = p[0].getWeight(); 00464 00465 double inc = 1.0/M; 00466 for (m = 0; m < M; m++){ 00467 while(u > c){ 00468 j++; 00469 if (j>=M) { 00470 // printf("[FastSLAM] ERROR!!!\n"); //Sleep(100000); 00471 } 00472 c += p[j].getWeight(); 00473 } 00474 resamp[m]=j; 00475 u += inc; 00476 } 00477 00478 double w0 = 1.0/M; 00479 for (i = 0; i < M; i++){ 00480 p[i].setWeight(w0); 00481 } 00482 00483 // RESAMPLING 00484 sampling.lock(); 00485 for (m = 0; m < M; m++){ 00486 newp[m] = p[resamp[m]]; 00487 } 00488 index = resamp[newindex]; 00489 particle* aux = p; 00490 p = newp; 00491 newp = aux; 00492 sampling.unlock(); 00493 00494 for (m = 0; m < M; m++) newp[m].releaseVMap(); 00495 00496 delete[] resamp; 00497 } 00498 return Neff; 00499 } 00500 00501 // Evaluates th uncertainty in the position for each robot 00502 void RBPFilter::evalDisp(int r){ 00503 int j; 00504 float sigma; 00505 float meanx, meany, meanth; 00506 double sum=M; 00507 00508 meanx=0; meany=0; meanth=0; 00509 for (j =0; j < M ; j++){ 00510 meanx += p[j].getPos(r).x ; 00511 meany += p[j].getPos(r).y ; 00512 meanth += p[j].getPos(r).th; 00513 //sum += p[j].getSumLogWeight(); 00514 } 00515 00516 meanx/= sum; meany/= sum; meanth/= sum; 00517 sigma = 0; 00518 for (j =0; j < M ; j++){ 00519 sigma += (pow(p[j].getPos(r).x - meanx,2) + pow(p[j].getPos(r).y - meany,2)); 00520 } 00521 sigma = sqrt(sigma/sum); 00522 00523 dispRobot[r] = sigma; 00524 00525 //printf("[FastSLAM 1] sigma = %f\n",sigma); 00526 // LOCALIZATION HYSTERESIS LOOP 00527 if (badlocalized[r] == false && sigma>th_high) badlocalized[r] = true; 00528 if (badlocalized[r] == true && sigma<th_low) badlocalized[r] = false; 00529 } 00530 00531 matrix RBPFilter::jacobian(const pose& pos){ 00532 00533 matrix res(3,3); 00534 00535 float costh = (float)cos(pos.th); 00536 float sinth = (float)sin(pos.th); 00537 00538 res.set(0,0,costh); 00539 res.set(0,1,sinth); 00540 res.set(1,0,-sinth); 00541 res.set(1,1,costh); 00542 res.set(2,2,1.0f); 00543 00544 return res; 00545 } 00546 00547 //TODO eval a covariance from the particles 00548 matrix RBPFilter::getCovariance(int robot) const{ 00549 matrix mat(3,3); 00550 return mat; 00551 }; 00552 00553 00554 00555 gridMapInterface* RBPFilter::getOMap() { 00556 sampling.lock(); 00557 gridMapInterface* map = (onlyonegridmap)? omap->clone() : p[index].getOMap().clone(); 00558 sampling.unlock(); 00559 return map; 00560 } 00561 00562 visualMap* RBPFilter::getVMap() { 00563 sampling.lock(); 00564 visualMap* map = p[index].getVMap().clone(); 00565 sampling.unlock(); 00566 return map; 00567 } 00568 00569 binMap* RBPFilter::getPreMap() { 00570 sampling.lock(); 00571 binMap* map = new binMap( (onlyonegridmap)? *ppmap : p[index].getPrecisePoseMap() ); 00572 sampling.unlock(); 00573 return map; 00574 } 00575 00576 binMap* RBPFilter::getImpMap() { 00577 sampling.lock(); 00578 binMap* map = new binMap( (onlyonegridmap)? *ipmap : p[index].getImprecisePoseMap() ); 00579 sampling.unlock(); 00580 return map; 00581 } 00582 00583 pose RBPFilter::getPos(int robot) { 00584 sampling.lock(); 00585 pose pos = p[index].getPos(robot); 00586 sampling.unlock(); 00587 return pos; 00588 } 00589 00590 point RBPFilter::getCell(int robot) { 00591 sampling.lock(); 00592 point c = p[index].getCell(robot); 00593 sampling.unlock(); 00594 return c; 00595 } 00596 00597 Ematrix RBPFilter::getGlobalCovariance() const{ 00598 00599 return Ematrix(); 00600 00601 } 00602 00603 QPixmap* RBPFilter::getPixmap(){ 00604 // IplImage* img = omap->getMapAsImage(); 00605 // cvSaveImage("imgoctmp.jpg",img); 00606 // cvReleaseImage(&img); 00607 // QPixmap* pix = new QPixmap("imgoctmp.jpg"); 00608 // return pix; 00609 00610 gridMapInterface* map = getOMap(); 00611 visualMap* vismap = getVMap(); 00612 00613 IplImage* img = map->getMapAsImage(); 00614 for (int r=0; r<nBots; r++) 00615 drawPoses (*img, xorigin, yorigin, resolution); 00616 vismap->drawMarks(*img,xorigin,yorigin,resolution); 00617 00618 delete map; 00619 delete vismap; 00620 00621 QImage* qim = new QImage((const uchar *)img->imageData, img->width, img->height, img->widthStep, QImage::Format_RGB888); 00622 QPixmap* mapfig = new QPixmap(QPixmap::fromImage(*qim)); 00623 cvReleaseImage(&img); 00624 delete qim; 00625 00626 return mapfig; 00627 00628 } 00629 00630 00631 void RBPFilter::drawPoses (IplImage& im, float xorigin, float yorigin, float resolution) { 00632 00633 CvScalar redcolor = CV_RGB(255,0,0); 00634 sampling.lock(); 00635 for (int r=0; r<nBots; r++){ 00636 for (int m = 0; m < M ; m++){ 00637 point cell = p[m].getCell(r); 00638 cvEllipse(&im, cvPoint(cell.x,im.height-cell.y), cvSize(0,0), 0, 0, 360, redcolor, CV_FILLED ); 00639 } 00640 } 00641 sampling.unlock(); 00642 } 00643
1.7.6.1
