|
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 OGMReflectProb 00008 * 00009 * Implements an ocupancy grid map 00010 * 00011 * 00012 * 00013 */ 00014 00015 #include <math.h> 00016 #include <stdio.h> 00017 #include <opencv2/opencv.hpp> 00018 #include "OGMReflectProb.h" 00019 #include <iostream> 00020 00021 00022 // Para registrar la clase en la factoria 00023 namespace{ 00024 gridMapInterface* CreateReflectProb(float w, float h, float res, float x, float y){ 00025 return new OGMReflectProb(w,h,res,x,y); 00026 }; 00027 const int REFLECTPROB = 3; 00028 const bool registered = gridMapFactory::Instance().Register(REFLECTPROB, gridMapCreator(CreateReflectProb)); 00029 } 00030 00031 using namespace std; 00032 00033 // Constructors 00034 OGMReflectProb::OGMReflectProb(): 00035 occupancyGridMap(), 00036 cells(0), 00037 step(1.0f) 00038 { 00039 }; 00040 00041 OGMReflectProb::OGMReflectProb(float w, float h, float res, float x, float y): 00042 occupancyGridMap(w,h,res,x,y), 00043 totalsize(width*height), 00044 cells(new cell[totalsize]), 00045 step(1.0f) 00046 { 00047 reset(); 00048 } 00049 00050 OGMReflectProb::~OGMReflectProb(){ 00051 if (cells) delete[] cells; 00052 } 00053 00054 OGMReflectProb::OGMReflectProb(const OGMReflectProb& gmap): 00055 occupancyGridMap(gmap.realWidth, gmap.realHeight, gmap.resolution, gmap.xorigin, gmap.yorigin), 00056 totalsize(gmap.totalsize), 00057 cells (new cell[gmap.totalsize]) 00058 { 00059 memcpy(cells,gmap.cells,totalsize*sizeof(cell)); 00060 } 00061 00062 gridMapInterface& OGMReflectProb::operator=(const gridMapInterface& gmap){ 00063 assert(typeid(gmap) == typeid(*this)); 00064 const OGMReflectProb& map = dynamic_cast<const OGMReflectProb&>(gmap); 00065 occupancyGridMap::operator=(gmap); 00066 if (totalsize != map.totalsize){ 00067 totalsize = map.totalsize; 00068 if (cells) delete[] cells; 00069 cells = new cell[totalsize]; 00070 } 00071 memcpy(cells,map.cells,totalsize*sizeof(cell)); 00072 return *this; 00073 } 00074 00075 // Initializer 00076 void OGMReflectProb::initialize(float w, float h, float res, float x, float y){ 00077 width = (int)floor(w/res+0.5); 00078 height = (int)floor(h/res+0.5); 00079 realWidth = w; 00080 realHeight = h; 00081 xorigin = x; 00082 yorigin = y; 00083 resolution = res; 00084 if (cells) delete[] cells; 00085 totalsize =width*height; 00086 cells = new cell[totalsize]; 00087 reset(); 00088 }; 00089 00090 // Data operations 00091 void OGMReflectProb::reset(){ 00092 step=1.0f; 00093 for(int i=0;i<totalsize;i++){ 00094 cells[i].occ = 0; 00095 cells[i].total = 0; 00096 cells[i].val = 50.0f; 00097 //cells[i].val2 = 50.0f; 00098 } 00099 } 00100 00101 // TODO:SONAR AND LASER MODEL ARE DIFERENT!!! 00102 void OGMReflectProb::update(const rangeSensorData& rsData, const pose& rpos, float disp){ 00103 00104 bool laser=true; 00105 if (laser){ 00106 point rcell,pcell; 00107 pointf p; 00108 int size, markCells; 00109 const int timeHorizon = 33; 00110 00111 float markingRange = rsData.getMaxDist() - (rsData.getDevError()*rsData.getMaxDist());; 00112 00113 rcell= toCell(rpos.x, rpos.y); // robot cell 00114 00115 // for each sensor 00116 for(int s=0; s < rsData.getNumSensors(); s++){ 00117 // position of the sensed point 00118 float th = rpos.th + rsData.getSensorPose(s).th; 00119 p.x = rpos.x + rsData.getSensorValue(s)*cos(th); 00120 p.y = rpos.y + rsData.getSensorValue(s)*sin(th); 00121 pcell = toCell(p.x, p.y); 00122 point* line = getLine(rcell.x,rcell.y,pcell.x,pcell.y,size); 00123 00124 markCells = floor((rsData.getDevError()*rsData.getSensorValue(s))/resolution)+1; 00125 00126 // clearing 00127 for (int i = 0; i < size-markCells; i++){ 00128 if (get(line[i].x,line[i].y).total < timeHorizon) 00129 get(line[i].x,line[i].y).total += 1.0f; 00130 else if (get(line[i].x,line[i].y).occ > 1) 00131 get(line[i].x,line[i].y).occ -= 1.0f; 00132 //get(line[i].x,line[i].y).val = 100.0f*get(line[i].x,line[i].y).occ/get(line[i].x,line[i].y).total; 00133 float valtmp = 100.0f*get(line[i].x,line[i].y).occ/get(line[i].x,line[i].y).total; 00134 get(line[i].x,line[i].y).val = valtmp; 00135 } 00136 delete line; 00137 } 00138 for(int s=0; s < rsData.getNumSensors(); s++){ 00139 // position of the sensed point 00140 float th = rpos.th + rsData.getSensorPose(s).th; 00141 p.x = rpos.x + rsData.getSensorValue(s)*cos(th); 00142 p.y = rpos.y + rsData.getSensorValue(s)*sin(th); 00143 pcell = toCell(p.x, p.y); 00144 point* line = getLine(rcell.x,rcell.y,pcell.x,pcell.y,size); 00145 00146 markCells = floor((rsData.getDevError()*rsData.getSensorValue(s))/resolution)+1; 00147 00148 // marking 00149 if (rsData.getSensorValue(s) < markingRange){ 00150 for (int i = size-markCells; i < size ; i++){ 00151 if (get(line[i].x,line[i].y).total < timeHorizon){ 00152 get(line[i].x,line[i].y).total += 1.0f; 00153 get(line[i].x,line[i].y).occ += 1.0f; 00154 } 00155 else if (get(line[i].x,line[i].y).occ < timeHorizon){ 00156 get(line[i].x,line[i].y).occ += 1.0f; 00157 } 00158 //get(line[i].x,line[i].y).val = 100.0f*get(line[i].x,line[i].y).occ/get(line[i].x,line[i].y).total; 00159 float valtmp = 100.0f*get(line[i].x,line[i].y).occ/get(line[i].x,line[i].y).total; 00160 get(line[i].x,line[i].y).val = valtmp; 00161 } 00162 } 00163 delete line; 00164 } 00165 } 00166 else{ 00167 //float conffactor = 1.0f; 00168 00169 int s,x,y; 00170 float xini, xend, yini, yend; 00171 int ixini, ixend, iyini, iyend; 00172 pose pSens, pSensRel; 00173 pointf p1, p2, p3; 00174 float d, xposdif, yposdif, ang, apmax, dmax, dmin, eps; //er, ea ; 00175 float auxsin, auxcos, auxemp, auxocc; 00176 float difangle; 00177 00178 apmax = rsData.getAperture()/2; 00179 dmax = rsData.getMaxDist(); 00180 dmin = rsData.getMinDist(); 00181 eps = 3*rsData.getDevError(); 00182 00183 double limval = dmax-eps-3*resolution; // 3*sigma 00184 double limvalmin = dmin+resolution; 00185 00186 // clear the position of the robot 00187 // TODO: clear the complete footprint => 00188 // - read the footprint from the config file 00189 // - draw lines for the footprint poligon 00190 // - filling algorithm 00191 // - translate it to the occupancy gridmap 00192 int rx = (int) floor((rpos.x - xorigin)/resolution); 00193 int ry = (int) floor((rpos.y - yorigin)/resolution); 00194 get(rx,ry).total++; 00195 get(rx,ry).val = 100.0f*get(rx,ry).occ/get(rx,ry).total; 00196 if (get(rx,ry).val == 50.0f) get(rx,ry).val = 49.0f; 00197 00198 for(s=0; s < rsData.getNumSensors(); s++){ // for each sensor 00199 00200 // position of the sensor 00201 auxcos = cos(rpos.th); 00202 auxsin = sin(rpos.th); 00203 00204 pSens.x = rpos.x + rsData.getSensorPose(s).x * auxcos - rsData.getSensorPose(s).y * auxsin; 00205 pSens.y = rpos.y + rsData.getSensorPose(s).x * auxsin + rsData.getSensorPose(s).y * auxcos; 00206 pSens.th = rpos.th + rsData.getSensorPose(s).th; 00207 00208 auxemp = (rsData.getSensorValue(s) - eps); 00209 auxocc = (rsData.getSensorValue(s) + eps); 00210 //float aux0 = auxemp-dmin; 00211 00212 // cone limits 00213 difangle = pSens.th + apmax; // one extreme of the beam 00214 auxcos = cos(difangle); 00215 auxsin = sin(difangle); 00216 p3.x = pSens.x + auxocc * auxcos; 00217 p3.y = pSens.y + auxocc * auxsin; 00218 difangle = pSens.th - apmax; // the other 00219 auxcos = cos(difangle); 00220 auxsin = sin(difangle); 00221 p1.x = pSens.x + auxocc * auxcos; 00222 p1.y = pSens.y + auxocc * auxsin; 00223 difangle = pSens.th; // the center 00224 auxcos = cos(difangle); 00225 auxsin = sin(difangle); 00226 p2.x = pSens.x + auxocc * auxcos; 00227 p2.y = pSens.y + auxocc * auxsin; 00228 00229 // cogemos el mayor y el menor para definir un area rectangular que actualizar 00230 xini = (pSens.x < p3.x)? ((pSens.x < p2.x)? ((pSens.x < p1.x)? pSens.x : p1.x): 00231 ((p2.x < p1.x)? p2.x : p1.x) ) : 00232 ((p3.x < p2.x)? ((p3.x < p1.x)? p3.x : p1.x): 00233 ((p2.x < p1.x)? p2.x : p1.x) ) ; 00234 xend = (pSens.x > p3.x)? ((pSens.x > p2.x)? ((pSens.x > p1.x)? pSens.x : p1.x): 00235 ((p2.x > p1.x)? p2.x : p1.x) ) : 00236 ((p3.x > p2.x)? ((p3.x > p1.x)? p3.x : p1.x): 00237 ((p2.x > p1.x)? p2.x : p1.x) ) ; 00238 00239 yini = (pSens.y < p3.y)? ((pSens.y < p2.y)? ((pSens.y < p1.y)? pSens.y : p1.y): 00240 ((p2.y < p1.y)? p2.y : p1.y) ) : 00241 ((p3.y < p2.y)? ((p3.y < p1.y)? p3.y : p1.y): 00242 ((p2.y < p1.y)? p2.y : p1.y) ) ; 00243 yend = (pSens.y > p3.y)? ((pSens.y > p2.y)? ((pSens.y > p1.y)? pSens.y : p1.y): 00244 ((p2.y > p1.y)? p2.y : p1.y) ) : 00245 ((p3.y > p2.y)? ((p3.y > p1.y)? p3.y : p1.y): 00246 ((p2.y > p1.y)? p2.y : p1.y) ) ; 00247 00248 // to cells 00249 ixini = (int) floor((xini - xorigin)/resolution)-1; 00250 ixend = (int) ceil ((xend - xorigin)/resolution)+1; 00251 iyini = (int) floor((yini - yorigin)/resolution)-1; 00252 iyend = (int) ceil ((yend - yorigin)/resolution)+1; 00253 00254 float posomapsensx = xorigin - pSens.x; 00255 float posomapsensy = yorigin - pSens.y; 00256 00257 //int sizex = ixend-ixini+1; 00258 //int sizey = iyend-iyini+1; 00259 //printf("size: %d x %d\n", sizex, sizey); 00260 00261 //float normalizesum = 0.0; 00262 00263 for(x=ixini; x<=ixend; x++){ 00264 for(y=iyini; y<=iyend; y++){ 00265 00266 xposdif = posomapsensx + resolution * (x+0.5f); 00267 yposdif = posomapsensy + resolution * (y+0.5f); 00268 ang = atan2(yposdif,xposdif) - pSens.th; 00269 if (ang>PImed) ang -= PIx2; 00270 if (ang<-PImed) ang += PIx2; 00271 ang = fabs(ang); 00272 00273 if (ang <= apmax){ // check angle 00274 d = sqrt(xposdif*xposdif+yposdif*yposdif); 00275 if (d <= limval && d >= limvalmin){ // check distance 00276 if (d < auxemp){ 00277 get(x,y).val = 0.0f; 00278 get(x,y).total++; 00279 } 00280 else if (d < auxocc){ // occupied zone 00281 get(x,y).val = 100.0f; 00282 get(x,y).occ+=3; 00283 get(x,y).total+=3; 00284 } 00285 } 00286 /*if (d <= limval && d >= limvalmin){ // check distance 00287 if (d < auxemp) 00288 get(x,y).total++; 00289 else if (d < auxocc){ // occupied zone 00290 get(x,y).occ+=3; 00291 get(x,y).total+=3; 00292 } 00293 get(x,y).val = 100.0f*get(x,y).occ/get(x,y).total; 00294 if (get(x,y).val == 50.0f) get(x,y).val = 49.0f; 00295 }*/ 00296 } 00297 } //y 00298 } //x 00299 }// sensor 00300 } 00301 } 00302 00303 /// save a map to disk 00304 int OGMReflectProb::saveMapToFile(const char* file) const{ 00305 try{ 00306 ofstream outfile; 00307 outfile.open (file, ios::out | ios::binary); 00308 if (outfile.good()){ 00309 outfile.write((char *) &width, sizeof width); 00310 outfile.write((char *) &height, sizeof height); 00311 outfile.write((char *) &realWidth, sizeof realWidth); 00312 outfile.write((char *) &realHeight, sizeof realHeight); 00313 outfile.write((char *) &xorigin, sizeof xorigin); 00314 outfile.write((char *) &yorigin, sizeof yorigin); 00315 outfile.write((char *) &resolution, sizeof resolution); 00316 outfile.write((char *) &totalsize, sizeof totalsize); 00317 outfile.write((char *) cells, totalsize*sizeof(cell)); 00318 outfile.close(); 00319 return true; 00320 } 00321 else{ 00322 printf("error saving map file\n"); 00323 return false; 00324 } 00325 } 00326 catch(const ios::failure &problem) 00327 { 00328 cout << problem.what(); 00329 return -1; 00330 } 00331 } 00332 00333 /// load a map from disk 00334 int OGMReflectProb::loadMapFromFile(const char* file){ 00335 try{ 00336 ifstream inputfile; 00337 inputfile.open(file, ios::in | ios::binary); 00338 if (inputfile.good()){ 00339 inputfile.read((char *) &width, sizeof width); 00340 inputfile.read((char *) &height, sizeof height); 00341 inputfile.read((char *) &realWidth, sizeof realWidth); 00342 inputfile.read((char *) &realHeight, sizeof realHeight); 00343 inputfile.read((char *) &xorigin, sizeof xorigin); 00344 inputfile.read((char *) &yorigin, sizeof yorigin); 00345 inputfile.read((char *) &resolution, sizeof resolution); 00346 inputfile.read((char *) &totalsize, sizeof totalsize); 00347 if (cells) delete[] cells; 00348 cells = new cell[totalsize](); 00349 inputfile.read((char *) cells, totalsize*sizeof(cell)); 00350 inputfile.close(); 00351 return true; 00352 } 00353 else{ 00354 printf("error loading map file\n"); 00355 return false; 00356 } 00357 } 00358 catch(const ios::failure &problem) 00359 { 00360 cout << problem.what(); 00361 return -1; 00362 } 00363 } 00364 00365
1.7.6.1
