grid-map.cpp
Go to the documentation of this file.
00001 #include "ethzasl_gridmap_2d/grid-map.h"
00002 #include "ethzasl_gridmap_2d/grid-functors.h"
00003 #include "ros/time.h"
00004 #include <cassert>
00005 #include <cstdlib>
00006 #include <algorithm>
00007 #include <queue>
00008 #include <fstream>
00009 #include <iostream>
00010 #include <map>
00011 #include <vector>
00012 #include <deque>
00013 #include <set>
00014 #include <limits>
00015 #include <stdexcept>
00016 #include <iostream>
00017 
00018 GridMap::GridMap(Group* mapGroup, const Value defaultValue) :
00019         resolution(0),
00020         startX(0),
00021         startY(0),
00022         width(0),
00023         height(0),
00024         defaultValue(defaultValue),
00025         mapGroup(mapGroup),
00026         rayCount(0)
00027 {
00028         assert(mapGroup);
00029         if (mapGroup->empty())
00030                 throw MapGroupEmpty();
00031         mapGroup->insert(this);
00032         const GridMap& that(**(mapGroup->begin()));
00033         resolution = that.resolution;
00034         startX = that.startX;
00035         startY = that.startY;
00036         width = that.width;
00037         height = that.height;
00038         values.resize(width*height, defaultValue);
00039 }
00040 
00042 GridMap::GridMap(const float resolution, const Value defaultValue, Group* mapGroup) :
00043         resolution(resolution),
00044         startX(0),
00045         startY(0),
00046         width(0),
00047         height(0),
00048         defaultValue(defaultValue),
00049         mapGroup(mapGroup),
00050         rayCount(0)
00051 {
00052         initiateMapGroup();
00053 }
00054 
00056 GridMap::GridMap(const float resolution, const float startX, const float startY, const float width, const float height, const Value defaultValue, Group* mapGroup) :
00057         resolution(resolution),
00058         startX(startX / resolution),
00059         startY(startY / resolution),
00060         width(width / resolution),
00061         height(height / resolution),
00062         defaultValue(defaultValue),
00063         values(width*height, defaultValue),
00064         mapGroup(mapGroup),
00065         rayCount(0)
00066 {
00067         initiateMapGroup();
00068 }
00069 
00071 GridMap::GridMap(const std::string &pgmFileName, const float resolution, const Value defaultValue, Group* mapGroup) :
00072         resolution(resolution),
00073         startX(0),
00074         startY(0),
00075         defaultValue(defaultValue),
00076         mapGroup(mapGroup),
00077         rayCount(0)
00078 {
00079         initiateMapGroup();
00080         
00081         // TODO: move this into a function and add support for loading a map already in a group
00082         std::ifstream ifs(pgmFileName.c_str(), std::ifstream::in);
00083         if (!ifs.good())
00084                 throw std::runtime_error("Cannot open file " + pgmFileName);
00085         std::string magic;
00086         ifs >> magic;
00087         if (magic != "P2")
00088                 throw std::runtime_error("Not a PGM file: " + pgmFileName);
00089         ifs >> width;
00090         ifs >> height;
00091         int valuesScale;
00092         ifs >> valuesScale;
00093         values.reserve(width*height);
00094         for (int y = 0; y < height; ++y)
00095         {
00096                 for (int x = 0; x < width; ++x)
00097                 {
00098                         int value;
00099                         ifs >> value;
00100                         if (ifs.eof())
00101                                 throw std::runtime_error("Early end-of-file: " + pgmFileName);
00102                         values.push_back(Value((value * 65535) / valuesScale - 32768));
00103                 }
00104         }
00105 }
00106 
00107 void GridMap::initiateMapGroup()
00108 {
00109         if (mapGroup)
00110         {
00111                 if (!mapGroup->empty())
00112                         throw MapGroupNotEmpty();
00113                 mapGroup->insert(this);
00114         }
00115 }
00116 
00117 GridMap::GridMap(const GridMap& that) :
00118         resolution(that.resolution),
00119         startX(that.startX),
00120         startY(that.startY),
00121         width(that.width),
00122         height(that.height),
00123         defaultValue(that.defaultValue),
00124         values(that.values),
00125         mapGroup(that.mapGroup),
00126         rayCount(0)
00127 {
00128         if (mapGroup)
00129                 mapGroup->insert(this);
00130 }
00131 
00132 GridMap& GridMap::operator=(const GridMap& that)
00133 {
00134         resolution = that.resolution;
00135         startX = that.startX;
00136         startY = that.startY;
00137         width = that.width;
00138         height = that.height;
00139         defaultValue = that.defaultValue;
00140         values = that.values;
00141         if (mapGroup)
00142                 mapGroup->erase(this);
00143         mapGroup = that.mapGroup;
00144         if (mapGroup)
00145                 mapGroup->insert(this);
00146         rayCount = 0;
00147         return *this;
00148 }
00149 
00150 GridMap::~GridMap()
00151 {
00152         // find in group and delete
00153         if (mapGroup)
00154                 mapGroup->erase(this);
00155 }
00156 
00157 GridMap::Value& GridMap::atInternalCoord(const int x, const int y)
00158 {
00159         assert(isWithinBoundsInternal(x,y));
00160         return values[y * width + x];
00161 }
00162 
00163 GridMap::Value GridMap::atInternalCoord(const int x, const int y) const 
00164 {
00165         assert(isWithinBoundsInternal(x,y));
00166         return values[y * width + x];
00167 }
00168 
00169 GridMap::Vector GridMap::fromInternalCoord(const int x, const int y) const
00170 {
00171         const float halfResolution(resolution / 2);
00172         return Vector((x + startX) * resolution + halfResolution, (y + startY) * resolution + halfResolution);
00173 }
00174 
00175 GridMap::Vector GridMap::fromInternalCoord(const float x, const float y) const
00176 {
00177         const float halfResolution(resolution / 2);
00178         return Vector((x + startX) * resolution + halfResolution, (y + startY) * resolution + halfResolution);
00179 }
00180 
00181 void GridMap::toInternalCoord(const Vector& pos, int& internalX, int& internalY) const
00182 {
00183         internalX = pos.x() / resolution - startX;
00184         internalY = pos.y() / resolution - startY;
00185 }
00186 
00187 void GridMap::toInternalCoordSuperSampled(const Vector& pos, const int superSampleRes, int& internalX, int& internalY) const 
00188 {
00189         internalX = (pos.x() * superSampleRes) / resolution - (startX * superSampleRes);
00190         internalY = (pos.y() * superSampleRes) / resolution - (startY * superSampleRes);
00191 }
00192 
00193 GridMap::Value GridMap::getValueNearest(const Vector& pos) const
00194 {
00195         int x, y;
00196         toInternalCoord(pos, x, y);
00197         return atInternalCoord(x,y);
00198 }
00199 
00200 void GridMap::setNearestValue(const Vector& pos, const Value value)
00201 {
00202         int x, y;
00203         toInternalCoord(pos, x, y);
00204         atInternalCoord(x,y) = value;
00205 }
00206 
00207 void GridMap::addNearestValueSaturated(const Vector& pos, const int delta)
00208 {
00209         int x, y;
00210         toInternalCoord(pos, x, y);
00211         Value &v(atInternalCoord(x,y));
00212         v = saturatedValueFromInt(int(v) + delta);
00213 }
00214 
00215 GridMap::Value GridMap::getValue(const Vector& pos) const
00216 {
00217         int x, y;
00218         toInternalCoord(pos, x, y);
00219         
00220         // we cannot make interpolation if we do not have the four points, so we return the non-interpolated point
00221         if ((x >= width - 1) || (y >= height - 1))
00222                 return atInternalCoord(x,y);
00223         
00224         // interpolation
00225         const float halfResolution(resolution / 2);
00226         const Vector d((pos - fromInternalCoord(x, y) + Vector(halfResolution,halfResolution)) / float(resolution));
00227         const float vx0y0(atInternalCoord(x,y));
00228         const float vx0y1(atInternalCoord(x,y+1));
00229         const float vx1y0(atInternalCoord(x+1,y));
00230         const float vx1y1(atInternalCoord(x+1,y+1));
00231         const float vxNy0(vx0y0 + d.x() * (vx1y0-vx0y0));
00232         const float vxNy1(vx0y1 + d.x() * (vx1y1-vx0y1));
00233         return vxNy0 + d.y() * (vxNy1-vxNy0);
00234 }
00235 
00236 GridMap::Vector GridMap::getSlope(const Vector& pos, const float limit) const
00237 {
00238         int x, y;
00239         toInternalCoord(pos, x, y);
00240         
00241         // we cannot make slope if we do not have the four points
00242         if ((x >= width - 1) || (y >= height - 1))
00243                 return Vector(0,0);
00244         
00245         const float halfResolution(resolution / 2);
00246         const Vector d((pos - fromInternalCoord(x, y) + Vector(halfResolution,halfResolution)) / float(resolution));
00247         const float vx0y0(atInternalCoord(x,y));
00248         const float vx0y1(atInternalCoord(x,y+1));
00249         const float vx1y0(atInternalCoord(x+1,y));
00250         const float vx1y1(atInternalCoord(x+1,y+1));
00251         const float vdxy0(std::min(std::max(vx1y0 - vx0y0, -limit), limit));
00252         const float vdxy1(std::min(std::max(vx1y1 - vx0y1, -limit), limit));
00253         const float vx0dy(std::min(std::max(vx0y1 - vx0y0, -limit), limit));
00254         const float vx1dy(std::min(std::max(vx1y1 - vx1y0, -limit), limit));
00255         return Vector(
00256                 d.y() * vdxy1 + (1 - d.y()) * vdxy0,
00257                 d.x() * vx1dy + (1 - d.x()) * vx0dy
00258         );
00259 }
00260 
00261 bool GridMap::isWithinBoundsInternal(const int x, const int y) const
00262 {
00263         return (x >= 0) && (y >= 0) && (x < width) && (y < height);
00264 }
00265 
00266 bool GridMap::isWithinBounds(const Vector& pos) const
00267 {
00268         int x,y;
00269         toInternalCoord(pos, x, y);
00270         return isWithinBoundsInternal(x, y);
00271 }
00272 
00273 void GridMap::extendToFit(const Vector& pos)
00274 {
00275         int x, y;
00276         toInternalCoordSuperSampled(pos, 256, x, y);
00277         extendMap(x / 256 - 1, y / 256 - 1, x / 256, y / 256);
00278 }
00279 
00280 nav_msgs::OccupancyGrid GridMap::toOccupancyGrid(const std::string& frame_id, const GridMap* knownMap) const
00281 {
00282         if (knownMap)
00283         {
00284                 if (!mapGroup || (mapGroup != knownMap->mapGroup))
00285                         throw WrongKnownMap();
00286         }
00287         
00288         nav_msgs::OccupancyGrid og;
00289         
00290         og.header.stamp = ros::Time::now();
00291         og.header.frame_id = frame_id;
00292         
00293         og.info.map_load_time = ros::Time::now();
00294         og.info.resolution = float(resolution);
00295         og.info.width = width;
00296         og.info.height = height;
00297         og.info.origin.position.x = float(startX) * og.info.resolution;
00298         og.info.origin.position.y = float(startY) * og.info.resolution;
00299         og.info.origin.position.z = 0;
00300         og.info.origin.orientation.x = 0;
00301         og.info.origin.orientation.y = 0;
00302         og.info.origin.orientation.z = 0;
00303         og.info.origin.orientation.w = 1;
00304         
00305         assert(int(values.size()) == width * height);
00306         assert((!knownMap) || (knownMap->values.size() == values.size()));
00307         og.data.resize(width * height);
00308         for (size_t i = 0; i < values.size(); ++i)
00309         {
00310                 if (knownMap && (knownMap->values[i] == -1))
00311                         og.data[i] = -1;
00312                 else
00313                         og.data[i] = ((int(values[i]) + 32768) * 100) / 65535;
00314         }
00315         
00316         return og;
00317 }
00318 
00319 // line update function, with sub-pixel accuracy
00320 
00321 template<typename F>
00322 void GridMap::lineScan(const Vector& start, const Vector& stop, F& functor, const Value& value)
00323 {
00324         Value values[2] = {value, value};
00325         lineScan<F>(start, stop, functor, values, 2);
00326 }
00327 
00328 template<typename F>
00329 void GridMap::lineScan(const Vector& start, const Vector& stop, F& functor, const Value texture[], const unsigned textureLength)
00330 {
00331         int x0, y0, x1, y1;
00332 
00333         rayCount++;
00334 
00335         toInternalCoordSuperSampled(start, 256, x0, y0);
00336         toInternalCoordSuperSampled(stop, 256, x1, y1);
00337         
00338         // see whether we should extend map, and if so, reget coordinates
00339         // we remove 1 to cope with the fact that ]-1:1[ -> 0 when subsampling
00340         if (extendMap(std::min(x0,x1) / 256 - 1, std::min(y0,y1) / 256 - 1, std::max(x0,x1) / 256, std::max(y0,y1) / 256))
00341         {
00342                 toInternalCoordSuperSampled(start, 256, x0, y0);
00343                 toInternalCoordSuperSampled(stop, 256, x1, y1);
00344         }
00345         
00346         // TODO: remove once we are confident in resize code
00347         assert(x0 >= 0);
00348         assert(x0 >> 8 < width);
00349         assert(x1 >= 0);
00350         assert(x1 >> 8 < width);
00351         assert(y0 >= 0);
00352         assert(y0 >> 8 < height);
00353         assert(y1 >= 0);
00354         assert(y1 >> 8 < height);
00355         
00356         const bool steep(abs(y1 - y0) > abs(x1 - x0));
00357         if (steep)
00358         {
00359                 std::swap(x0, y0);
00360                 std::swap(x1, y1);
00361         }
00362         assert(textureLength > 1);
00363         const int maxTexIndex(textureLength - 1);
00364         const int deltatex((maxTexIndex << 16)/(x1 - x0));
00365         
00366         bool reverseScan;
00367         if (x0 > x1)
00368         {
00369                 std::swap(x0, x1);
00370                 std::swap(y0, y1);
00371                 reverseScan = true;
00372         }
00373         else
00374                 reverseScan = false;
00375         const int deltax(x1 - x0);
00376         const int deltay(y1 - y0);
00377         
00378         // we now compute the sub-pixel displacement on the texture
00379         const int subx0( (x0 & 0xff) - 128);
00380         const int suby0( (y0 & 0xff) - 128);
00381         const int t((deltay * suby0) / deltax);
00382         const int xp(subx0 + t);
00383         const int hypSlope(sqrtf(deltay*deltay + deltax*deltax));
00384         const int texDiff((xp * deltax) / hypSlope);
00385         //cerr << "(" << x0 << "," << y0 << ") (" << deltax << "," << deltay << ") (" << subx0 << "," << suby0 << ") "<< t << " " << xp << " " << hypSlope << endl;
00386         //cerr << "resulting tex diff " << texDiff << endl;
00387         const int maxTex(textureLength << 8);
00388         int tex;
00389         if (deltatex >= 0)
00390         {
00391                 tex = 0;
00392         }
00393         else
00394         {
00395                 tex = maxTexIndex << 8;
00396         }
00397         // correct pixels aliasing
00398         tex -= (deltatex * texDiff) >> 8;
00399         // correct texture aliasing
00400         tex += 128;
00401         
00402         int y(y0);
00403         const int ystep = (deltay << 8) / deltax;
00404         
00405         // TODO: if required, optimize this with direct pointers, however the compiler should do so.
00406         
00407         bool functorRet;
00408         
00409         // first point may be outside texture
00410         if ((tex >= 0) && (tex < maxTex))
00411         {
00412                 // apply functor
00413                 if (steep)
00414                         functorRet = functor(y >> 8, x0 >> 8, texture[tex >> 8], reverseScan);
00415                 else
00416                         functorRet = functor(x0 >> 8, y >> 8, texture[tex >> 8], reverseScan);
00417                 // check whether we have an early stop
00418                 if (!functorRet)
00419                         return;
00420         }
00421         // increment texture and y position
00422         tex += deltatex;
00423         y += ystep;
00424         // x is main counter
00425         int x = x0 + 256;
00426         for (; x < x1 - 256; x += 256)
00427         {
00428                 
00429                 // apply functor
00430                 if (steep)
00431                         functorRet = functor(y >> 8, x >> 8, texture[tex >> 8], reverseScan);
00432                 else
00433                         functorRet = functor(x >> 8, y >> 8, texture[tex >> 8], reverseScan);
00434                 // check whether we have an early stop
00435                 if (!functorRet)
00436                         return;
00437                 assert(tex < maxTex);
00438                 // increment texture and y position
00439                 tex += deltatex;
00440                 y += ystep;
00441         }
00442         // last point may be outside texture
00443         if ((tex >= 0) && (tex < maxTex))
00444         {
00445                 // apply functor
00446                 if (steep)
00447                         functor(y >> 8, x >> 8, texture[tex >> 8], reverseScan);
00448                 else
00449                         functor(x >> 8, y >> 8, texture[tex >> 8], reverseScan);
00450         }
00451 }
00452 
00453 // force instantiation of these templates
00454 template void GridMap::lineScan<MapCorrelation>(const Vector& start, const Vector& stop, MapCorrelation& functor, const Value& value);
00455 template void GridMap::lineScan<MapUpdater>(const Vector& start, const Vector& stop, MapUpdater& functor, const Value& value);
00456 template void GridMap::lineScan<MapConstUpdater>(const Vector& start, const Vector& stop, MapConstUpdater& functor, const Value& value);
00457 template void GridMap::lineScan<MapWallFinder>(const Vector& start, const Vector& stop, MapWallFinder& functor, const Value& value);
00458 template void GridMap::lineScan<MapEndOfAreaFinder>(const Vector& start, const Vector& stop, MapEndOfAreaFinder& functor, const Value& value);
00459 template void GridMap::lineScan<Drawer>(const Vector& start, const Vector& stop, Drawer& functor, const Value& value);
00460 
00461 
00462 bool GridMap::extendMap(int xMin, int yMin, int xMax, int yMax)
00463 {
00464         // sanity check
00465         if (mapGroup)
00466         {
00467                 // we assume that all maps of group are already in sync
00468                 for (Group::iterator it = mapGroup->begin(); it != mapGroup->end(); ++it)
00469                 {
00470                         const GridMap& that(**it);
00471                         assert(that.startX == startX);
00472                         assert(that.startY == startY);
00473                         assert(that.width == width);
00474                         assert(that.height == height);
00475                 }
00476         }
00477         
00478         // extend coordinates
00479         int deltaStartX = 0;
00480         int deltaStartY = 0;
00481         if (xMin < 0)
00482         {
00483                 /*
00484                 // minimum extension
00485                 deltaStartX = xMin;
00486                 xMax -= deltaStartX;
00487                 xMin = 0;*/
00488                 // constant extension
00489                 deltaStartX = (xMin - 31) & (~0x1F);
00490                 xMax -= deltaStartX;
00491                 xMin -= deltaStartX;
00492                 assert(xMin >= 0);
00493         }
00494         if (yMin < 0)
00495         {
00496                 /*
00497                 // minimum extension
00498                 deltaStartY = yMin;
00499                 yMax -= deltaStartY;
00500                 yMin = 0;
00501                 */
00502                 // constant extension
00503                 deltaStartY = (yMin - 31) & (~0x1F);
00504                 yMax -= deltaStartY;
00505                 yMin -= deltaStartY;
00506                 assert(yMin >= 0);
00507         }
00508         /*
00509         // minimum extension
00510         const int newWidth = std::max(width - deltaStartX, xMax + 1);
00511         const int newHeight = std::max(height - deltaStartY, yMax + 1);
00512         */
00513         // constant extension
00514         int newWidth = width - deltaStartX;
00515         int newHeight = height - deltaStartY;
00516         if (newWidth < xMax + 1)
00517                 newWidth += (xMax + 1 - newWidth + 31) & (~0x1F);
00518         if (newHeight < yMax + 1)
00519                 newHeight += (yMax + 1 - newHeight + 31) & (~0x1F);
00520         
00521         // if nothing to do, return
00522         if ((deltaStartX == 0) && (deltaStartY == 0) && (newWidth == width) && (newHeight == height))
00523                 return false;
00524         
00525         // if we must extend the map, extend others as well
00526         if (mapGroup)
00527         {
00528                 // we assume that all maps of group are already in sync
00529                 for (Group::iterator it = mapGroup->begin(); it != mapGroup->end(); ++it)
00530                         (*it)->extendMapInternal(deltaStartX, deltaStartY, newWidth, newHeight);
00531         }
00532         else
00533                 extendMapInternal(deltaStartX, deltaStartY, newWidth, newHeight);
00534         
00535         return true;
00536 }
00537 
00538 void GridMap::extendMapInternal(int deltaStartX, int deltaStartY, int newWidth, int newHeight)
00539 {
00540         // create new map
00541         Values newValues(newWidth * newHeight, defaultValue);
00542         
00543         // copy, we know that we have enough room
00544         Values::const_iterator xitIn = values.begin();
00545         for (int y = 0; y < height; ++y)
00546         {
00547                 Values::iterator xitOut = newValues.begin() + (y - deltaStartY) * newWidth - deltaStartX;
00548                 for (int x = 0; x < width; ++x)
00549                 {
00550                         *xitOut++ = *xitIn++;
00551                 }
00552         }
00553         
00554         // exchange old map for new map 
00555         swap(values, newValues);
00556         
00557         // update dimensions
00558         startX += deltaStartX;
00559         startY += deltaStartY;
00560         width = newWidth;
00561         height = newHeight;
00562 }
00563 
00564 void GridMap::invert()
00565 {
00566         for (Values::iterator it = values.begin(); it != values.end(); ++it)
00567         {
00568                 const Value value(*it);
00569                 if (value > -32768)
00570                         *it = -value;
00571                 else
00572                         *it = 32767;
00573         }
00574 }
00575 
00576 void GridMap::threshold(const Value threshold, const Value lowValue, const Value highValue)
00577 {
00578         for (Values::iterator it = values.begin(); it != values.end(); ++it)
00579         {
00580                 const Value value(*it);
00581                 if (value >= threshold)
00582                         *it = highValue;
00583                 else
00584                         *it = lowValue;
00585         }
00586 }
00587 
00588 static const int lookupMap8[][2] = {
00589         { -1, -1 },
00590         { 0, -1 },
00591         { 1, -1 },
00592         { -1, 0 },
00593         { 1, 0 },
00594         { -1, 1 },
00595         { 0, 1 },
00596         { 1, 1 },
00597 };
00598 
00599 static const int lookupMap4[][2] = {
00600         { 0, -1 },              // top
00601         { -1, 0 },              // left
00602         { 1, 0 },               // right
00603         { 0, 1 },               // bottom
00604 };
00605 
00606 // TODO: when "amount" is large, these are not efficient. A wavefront-based algorithm would be faster, but would require more work to implement
00607 
00608 void GridMap::dilate4(const unsigned amount, const unsigned delta)
00609 {
00610         dilateN(amount, lookupMap4, 4, delta);
00611 }
00612 
00613 void GridMap::dilate8(const unsigned amount, const unsigned delta)
00614 {
00615         dilateN(amount, lookupMap8, 8, delta);
00616 }
00617 
00618 void GridMap::dilateN(const unsigned amount, const int lookup[][2], const size_t lookupSize, const unsigned delta)
00619 {
00620         Values newValues(values.size());
00621         for (unsigned pass = 0; pass < amount; ++pass)
00622         {
00623                 for (int y = 1; y < height - 1; ++y)
00624                 {
00625                         for (int x = 1; x < width - 1; ++x)
00626                         {
00627                                 // TODO: optimize by putting delta pixels into the lookup values
00628                                 Value newValue(values[y * width + x]);
00629                                 for (size_t i = 0; i < lookupSize; ++i)
00630                                         newValue = std::max<int>(newValue, int(atInternalCoord(x + lookup[i][0], y + lookup[i][1])) - delta);
00631                                 newValues[y * width + x] = newValue;
00632                         }
00633                 }
00634                 std::swap(values, newValues);
00635         }
00636 }
00637 
00638 void GridMap::erode4(const unsigned amount, const unsigned delta)
00639 {
00640         erodeN(amount, lookupMap4, 4, delta);
00641 }
00642 
00643 void GridMap::erode8(const unsigned amount, const unsigned delta)
00644 {
00645         erodeN(amount, lookupMap8, 8, delta);
00646 }
00647 
00648 void GridMap::erodeN(const unsigned amount, const int lookup[][2], const size_t lookupSize, const unsigned delta)
00649 {
00650         Values newValues(values.size());
00651         for (unsigned pass = 0; pass < amount; ++pass)
00652         {
00653                 for (int y = 1; y < height - 1; ++y)
00654                 {
00655                         for (int x = 1; x < width - 1; ++x)
00656                         {
00657                                 // TODO: optimize by putting delta pixels into the lookup values
00658                                 Value newValue(values[y * width + x]);
00659                                 for (size_t i = 0; i < lookupSize; ++i)
00660                                         newValue = std::min<int>(newValue, int(atInternalCoord(x + lookup[i][0], y + lookup[i][1])) + delta);
00661                                 newValues[y * width + x] = newValue;
00662                         }
00663                 }
00664                 std::swap(values, newValues);
00665         }
00666 }
00667 
00668 // A multimap that provides a method to push in unique key,value pairs
00669 template <typename Key, typename Value>
00670 class UniquePairsMultiMap: public std::multimap<Key, Value>
00671 {
00672 public:
00673         typedef std::multimap<Key, Value> MapType;
00674         void insertUniquePair(const Key& key, const Value& value)
00675         {
00676                 std::pair<typename MapType::iterator, typename MapType::iterator> range = this->equal_range (key);
00677                 if (range.first != range.second)
00678                 {
00679                         for (typename MapType::iterator it = range.first; it != range.second; ++it)
00680                                 if (it->second == value)
00681                                         return;
00682                 }
00683                 this->insert(std::pair<Key, Value>(key, value));
00684         }
00685         void erasePair(const Key& key, const Value& value)
00686         {
00687                 std::pair<typename MapType::iterator, typename MapType::iterator> range = this->equal_range (key);
00688                 for (typename MapType::iterator it = range.first; it != range.second; ++it)
00689                         if (it->second == value)
00690                         {
00691                                 this->erase(it);
00692                                 return;
00693                         }
00694         }
00695 };
00696 
00697 GridMap::Labels GridMap::labelize(const Value threshold)
00698 {
00699         if (width < 1 || height < 1)
00700                 return Labels();
00701         
00702         typedef long long Int64;
00703         typedef std::vector<Int64> Int64Vector;
00704         typedef UniquePairsMultiMap<Value,Value> EquivalentLabels;
00705         Int64Vector xs;
00706         Int64Vector ys;
00707         Int64Vector counters;
00708         EquivalentLabels equivalentLabels;
00709         Values labeledValues(values.size(), -1);
00710         
00711         // Part 1: segment area in address-increasing order, and note equivalent relations
00712         for (int y = 0; y < height; ++y)
00713         {
00714                 for (int x = 0; x < width; ++x)
00715                 {
00716                         const int index(y * width + x);
00717                         if (values[index] >= threshold)
00718                         {
00719                                 // four cases
00720                                 if (y > 0 && labeledValues[index-width] >= 0)
00721                                 {
00722                                         // top is labeled
00723                                         if (x > 0 && labeledValues[index-1] >= 0)
00724                                         {
00725                                                 // left is labeled
00726                                                 if (labeledValues[index-1] != labeledValues[index-width])
00727                                                 {
00728                                                         // note equivalent relation
00729                                                         equivalentLabels.insertUniquePair(labeledValues[index-width], labeledValues[index-1]);
00730                                                         equivalentLabels.insertUniquePair(labeledValues[index-1], labeledValues[index-width]);
00731                                                 }
00732                                         }
00733                                         // extend top label
00734                                         const Value label(labeledValues[index-width]);
00735                                         xs[label] += Int64(x); ys[label] += Int64(y); counters[label] += 1;
00736                                         labeledValues[index] = label;
00737                                 }
00738                                 else
00739                                 {
00740                                         // top is not labeled
00741                                         if (x > 0 && labeledValues[index-1] >= 0)
00742                                         {
00743                                                 // left is labeled
00744                                                 // extend left label
00745                                                 const Value label(labeledValues[index-1]);
00746                                                 xs[label] += Int64(x); ys[label] += Int64(y); counters[label] += 1;
00747                                                 labeledValues[index] = label;
00748                                         }
00749                                         else
00750                                         {
00751                                                 // left is not labeled
00752                                                 // new area
00753                                                 const Value label(counters.size());
00754                                                 assert(counters.size() < size_t(std::numeric_limits<Value>::max()));
00755                                                 xs.push_back(Int64(x)); ys.push_back(Int64(y)); counters.push_back(1);
00756                                                 labeledValues[index] = label;
00757                                         }
00758                                 }
00759                         }
00760                 }
00761         }
00762         
00763         // Part 2: fuse equivalent relations
00764         typedef std::vector<Value> LabelsLookup;
00765         LabelsLookup labelsLookup(counters.size(), -1);
00766         Int64Vector targetsXs;
00767         Int64Vector targetsYs;
00768         Int64Vector targetsCounters;
00769         while (!equivalentLabels.empty())
00770         {
00771                 // build a new queue and create target label
00772                 typedef std::deque<Value> LabelsQueue;
00773                 LabelsQueue workQueue;
00774                 workQueue.push_back(equivalentLabels.begin()->first);
00775                 const Value targetLabel(targetsCounters.size());
00776                 targetsXs.push_back(0);
00777                 targetsYs.push_back(0);
00778                 targetsCounters.push_back(0);
00779                 while (!workQueue.empty())
00780                 {
00781                         // process this one and its attached nodes
00782                         const Value current(workQueue.front());
00783                         workQueue.pop_front();
00784                         labelsLookup[current] = targetLabel;
00785                         targetsXs[targetLabel] += xs[current];
00786                         targetsYs[targetLabel] += ys[current];
00787                         targetsCounters[targetLabel] += counters[current];
00788                         std::pair<EquivalentLabels::iterator, EquivalentLabels::iterator> range = equivalentLabels.equal_range(current);
00789                         while (range.first != range.second)
00790                         {
00791                                 EquivalentLabels::iterator currentIt = range.first;
00792                                 // push to work queue
00793                                 workQueue.push_back(currentIt->second);
00794                                 // remove relation from graph
00795                                 equivalentLabels.erasePair(currentIt->second, currentIt->first);
00796                                 equivalentLabels.erase(currentIt);
00797                                 // get range again
00798                                 range = equivalentLabels.equal_range(current);
00799                         }
00800                 }
00801         }
00802         // copy labels with no equivalent relations
00803         for (size_t i = 0; i < labelsLookup.size(); ++i)
00804         {
00805                 if (labelsLookup[i] < 0)
00806                 {
00807                         const Value targetLabel(targetsCounters.size());
00808                         labelsLookup[i] = targetLabel;
00809                         targetsXs.push_back(xs[i]);
00810                         targetsYs.push_back(ys[i]);
00811                         targetsCounters.push_back(counters[i]);
00812                 }
00813         }
00814         
00815         // Part 3: labelize final image and build center of mass vectors
00816         // TODO: optimize with a single loop
00817         for (int y = 0; y < height; ++y)
00818         {
00819                 for (int x = 0; x < width; ++x)
00820                 {
00821                         const int index(y * width + x);
00822                         if (labeledValues[index] >= 0)
00823                                 values[index] = labelsLookup[labeledValues[index]];
00824                         else
00825                                 values[index] = -1;
00826                 }
00827         }
00828         
00829         Labels labels;
00830         labels.reserve(targetsCounters.size());
00831         for (size_t i = 0; i < targetsCounters.size(); ++i)
00832         {
00833                 labels.push_back(
00834                         boost::make_tuple(
00835                                 i,
00836                                 fromInternalCoord(
00837                                         float(targetsXs[i]) / float(targetsCounters[i]),
00838                                         float(targetsYs[i]) / float(targetsCounters[i])
00839                                 ),
00840                                 targetsCounters[i]
00841                         )
00842                 );
00843         }
00844         return labels;
00845 }
00846 
00848 
00849 static float uniformRand(void)
00850 {
00851         return float(rand())/RAND_MAX;
00852 }
00853 
00855 
00856 static float gaussianRand(float mean, float sigm)
00857 {
00858         // Generation using the Polar (Box-Mueller) method.
00859         // Code inspired by GSL, which is a really great math lib.
00860         // http://sources.redhat.com/gsl/
00861         // C++ wrapper available.
00862         // http://gslwrap.sourceforge.net/
00863         float r, x, y;
00864         
00865         // Generate random number in unity circle.
00866         do
00867         {
00868                 x = uniformRand()*2 - 1;
00869                 y = uniformRand()*2 - 1;
00870                 r = x*x + y*y;
00871         }
00872         while (r > 1.0 || r == 0);
00873         
00874         // Box-Muller transform.
00875         return sigm * y * sqrt (-2.0 * log(r) / r) + mean;
00876 }
00877 
00878 static GridMap::Vector getRandomPoint(const GridMap::Label& area, const int resolution)
00879 {
00880         return GridMap::Vector(
00881                 gaussianRand(area.get<1>().x(), float(resolution)*sqrtf(area.get<2>())/2),
00882                 gaussianRand(area.get<1>().y(), float(resolution)*sqrtf(area.get<2>())/2)
00883         );
00884 }
00885 
00886 GridMap::VectorPair GridMap::closestPoints(const Label& area0, const Label& area1, const unsigned monteCarloSteps) const
00887 {
00888         const Value id0(area0.get<0>());
00889         //const Vector c0(area0.get<1>());
00890         const Value id1(area1.get<0>());
00891         //const Vector c1(area1.get<1>());
00892         //const Vector pc0c1u((c1-c0).perp().unitary());
00893         float bestDist(std::numeric_limits<float>::max());
00894         VectorPair bestPoints;
00895         for (unsigned i = 0; i < monteCarloSteps; ++i)
00896         // only one step for now, as we use centers
00897         {
00898                 // two points
00899                 Vector p0, p1;
00900                 do
00901                 {
00902                         p0 = getRandomPoint(area0, resolution);
00903                 }
00904                 while (!isWithinBounds(p0) || getValueNearest(p0) != id0);
00905                 do
00906                 {
00907                         p1 = getRandomPoint(area1, resolution);
00908                 }
00909                 while (!isWithinBounds(p1) || getValueNearest(p1) != id1);
00910                 /*// use center for now
00911                 Vector p0 = area0.get<1>();
00912                 while (!isWithinBounds(p0) || getValueNearest(p0) != id0)
00913                         p0 = getRandomPoint(area0, resolution);
00914                 Vector p1 = area1.get<1>();
00915                 while (!isWithinBounds(p1) || getValueNearest(p1) != id1)
00916                         p1 = getRandomPoint(area1, resolution);
00917                 */
00918                 // NOTE:        We use ugly const cast before it allows us to have only a non const version of linescan
00919                 //                      We thus save code in return for some uglyness.
00920                 // find wall of area 0
00921                 MapEndOfAreaFinder eoa0Finder(*this, id0);
00922                 const_cast<GridMap&>(*this).lineScan(p0, p1, eoa0Finder);
00923                 if (eoa0Finder.eoaX >= 0 && eoa0Finder.eoaY >= 0)
00924                         p0 = fromInternalCoord(eoa0Finder.eoaX, eoa0Finder.eoaY);
00925                 // find wall of area 1
00926                 MapEndOfAreaFinder eoa1Finder(*this, id1);
00927                 const_cast<GridMap&>(*this).lineScan(p1, p0, eoa1Finder);
00928                 if (eoa1Finder.eoaX >= 0 && eoa1Finder.eoaY >= 0)
00929                         p1 = fromInternalCoord(eoa1Finder.eoaX, eoa1Finder.eoaY);
00930                 
00931                 // is it better ?
00932                 const float dist((p1-p0). squaredNorm());
00933                 /*const Vector pm((p1+p0)/2);
00934                 const Vector r(p0 - pm);
00935                 const float dC(fabsf(pc0c1u * r));
00936                 const float score(dist + dC / 5);*/
00937                 if (dist < bestDist)
00938                 {
00939                         bestDist = dist;
00940                         bestPoints.first = p0;
00941                         bestPoints.second = p1;
00942                 }
00943         }
00944         return bestPoints;
00945 }
00946 
00947 GridMap GridMap::buildGradient(const Vector& goal, bool &isSuccess) const
00948 {
00949         typedef std::pair<int,int> Cell;
00950         typedef std::queue<Cell> CellsQueue;
00951 
00952         // note: we consider 6 bits sub-precision, that leads us to 512 cells range of pathfinding
00953         const int sp(6);
00954         const int oneSp(1<<sp);
00955         const float oneSpF(oneSp);
00956         const Value maxValue(std::numeric_limits<Value>::max());
00957         GridMap gradientMap(resolution, startX, startY, width, height, maxValue);
00958         
00959         // goal
00960         int goalX, goalY;
00961         toInternalCoord(goal, goalX, goalY);
00962         if (atInternalCoord(goalX, goalY) == 0)
00963         {
00964                 std::cerr << "GridMap::buildGradient(" << goal << ") : cannot create, goal is an obstacle (internal coord =" << goalX << "," << goalY << ")" << std::endl;
00965                 isSuccess = false;
00966                 return gradientMap;
00967         }
00968         gradientMap.atInternalCoord(goalX, goalY) = 0;
00969         
00970         // build initial queue of cells
00971         CellsQueue *workQueue = new CellsQueue;
00972         // S
00973         if ((goalY > 0) && (atInternalCoord(goalX, goalY-1) != 0))
00974                 workQueue->push(Cell(goalX, goalY-1));
00975         // W
00976         if ((goalX > 0) && (atInternalCoord(goalX-1, goalY) != 0))
00977                 workQueue->push(Cell(goalX-1, goalY));
00978         // E
00979         if ((goalX < width - 1) && (atInternalCoord(goalX+1, goalY) != 0))
00980                 workQueue->push(Cell(goalX+1, goalY));
00981         // N
00982         if ((goalY < height - 1) && (atInternalCoord(goalX, goalY+1) != 0))
00983                 workQueue->push(Cell(goalX, goalY+1));
00984         
00985         if (workQueue->empty())
00986         {
00987                 std::cerr << "GridMap::buildGradient(" << goal << ") : goal is locked (internal coord =" << goalX << "," << goalY << ")" << std::endl;
00988         }
00989         
00990         // propagate
00991         while (!workQueue->empty())
00992         {
00993                 const Cell& cell(workQueue->front());
00994                 const int x(cell.first);
00995                 const int y(cell.second);
00996                 workQueue->pop();
00997                 
00998                 // get values of neightbours
00999                 int smallestValue(maxValue);
01000                 int secondSmallestValue(maxValue);
01001                 // S
01002                 if (y > 0)
01003                 {
01004                         const int value(gradientMap.atInternalCoord(x, y-1));
01005                         if (value < smallestValue)
01006                                 secondSmallestValue = smallestValue, smallestValue = value;
01007                         else
01008                                 secondSmallestValue = std::min(value, secondSmallestValue);
01009                 }
01010                 // W
01011                 if (x > 0)
01012                 {
01013                         const int value(gradientMap.atInternalCoord(x-1, y));
01014                         if (value < smallestValue)
01015                                 secondSmallestValue = smallestValue, smallestValue = value;
01016                         else
01017                                 secondSmallestValue = std::min(value, secondSmallestValue);
01018                 }
01019                 // E
01020                 if (x < width - 1)
01021                 {
01022                         const int value(gradientMap.atInternalCoord(x+1, y));
01023                         if (value < smallestValue)
01024                                 secondSmallestValue = smallestValue, smallestValue = value;
01025                         else
01026                                 secondSmallestValue = std::min(value, secondSmallestValue);
01027                 }
01028                 // N
01029                 if (y < height - 1)
01030                 {
01031                         const int value(gradientMap.atInternalCoord(x, y+1));
01032                         if (value < smallestValue)
01033                                 secondSmallestValue = smallestValue, smallestValue = value;
01034                         else
01035                                 secondSmallestValue = std::min(value, secondSmallestValue);
01036                 }
01037                 
01038                 // if we haven't found any parent wave, that means that something is wrong there
01039                 assert(smallestValue != maxValue);
01040                 
01041                 // compute new value, from Philippsen TR E* 06 but simplified for uniform cells
01042                 // switch to float for computation
01043                 const float F(float(atInternalCoord(x, y)) / 32767.f);
01044                 assert(F >= 0.f);
01045                 const float Ta(float(smallestValue) / oneSpF);
01046                 const float Tc(float(secondSmallestValue) / oneSpF);
01047                 float T;
01048                 if (Tc - Ta >= 1.f/F)
01049                 {
01050                         T = Ta + 1.f/F;
01051                 }
01052                 else
01053                 {
01054                         const float beta(Ta + Tc);
01055                         const float gamma(Ta*Ta + Tc*Tc - 1.f/(F*F));
01056                         T = 0.5f * (beta + sqrtf(beta*beta-2.f*gamma));
01057                 }
01058                 const int newValue = int(T * oneSpF);
01059                 
01060                 // update value
01061                 const int currentValue(gradientMap.atInternalCoord(x,y));
01062                 if (newValue >= currentValue)
01063                         continue;
01064                 //std::cout << "updating " << x << "," << y << "; old value = "<< currentValue << ", new value = " << newValue << std::endl;
01065                 gradientMap.atInternalCoord(x,y) = newValue;
01066                 
01067                 // propagate
01068                 // S
01069                 if ((y > 0) && (atInternalCoord(x, y-1) != 0))
01070                         workQueue->push(Cell(x, y-1));
01071                 // W
01072                 if ((x > 0) && (atInternalCoord(x-1, y) != 0))
01073                         workQueue->push(Cell(x-1, y));
01074                 // E
01075                 if ((x < width - 1) && (atInternalCoord(x+1, y) != 0))
01076                         workQueue->push(Cell(x+1, y));
01077                 // N
01078                 if ((y < height - 1) && (atInternalCoord(x, y+1) != 0))
01079                         workQueue->push(Cell(x, y+1));
01080         }
01081         
01082         delete workQueue;
01083         isSuccess = true;
01084         return gradientMap;
01085 }
01086 
01087 inline float binaryEntropy(const GridMap::Value v)
01088 {
01089         const float vf(v);
01090         const float pf(1/(1+expf(-vf/32768.f)));
01091         return -(pf * logf(pf) + (1-pf)*logf(1-pf));
01092 }
01093 
01094 float GridMap::getInformationQuantity() const
01095 {
01096         float i = 0;
01097         const float hPrior(binaryEntropy(0));
01098         for (Values::const_iterator it(values.begin()); it != values.end(); ++it)
01099                 i += hPrior - binaryEntropy(*it);
01100         return i;
01101 }
01102 
01103 void GridMap::toPGMFile(const std::string& fileName, const int divisorToPGM) const
01104 {
01105         std::ofstream file(fileName.c_str());
01106         if (!file.good())
01107         {
01108                 std::cerr << "Cannot open file " << fileName << " for writing." << std::endl;
01109                 return;
01110         }
01111         
01112         file << "P2\n";
01113         file << width << " " << height << "\n255\n";
01114         for (int y = 0; y < height; ++y)
01115         {
01116                 for (int x = 0; x < width; ++x)
01117                         file << (int(atInternalCoord(x,y)) / divisorToPGM + 128) << " ";
01118                 file << "\n";
01119         }
01120 }


ethzasl_gridmap_2d
Author(s): Stéphane Magnenat
autogenerated on Thu Jan 2 2014 11:16:00