TopologicalMap.C

Go to the documentation of this file.
00001 /*!@file Beobot/TopologicalMap.C topological map                        */
00002 // //////////////////////////////////////////////////////////////////// //
00003 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00004 // University of Southern California (USC) and the iLab at USC.         //
00005 // See http://iLab.usc.edu for information about this project.          //
00006 // //////////////////////////////////////////////////////////////////// //
00007 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00008 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00009 // in Visual Environments, and Applications'' by Christof Koch and      //
00010 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00011 // pending; application number 09/912,225 filed July 23, 2001; see      //
00012 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00013 // //////////////////////////////////////////////////////////////////// //
00014 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00015 //                                                                      //
00016 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00017 // redistribute it and/or modify it under the terms of the GNU General  //
00018 // Public License as published by the Free Software Foundation; either  //
00019 // version 2 of the License, or (at your option) any later version.     //
00020 //                                                                      //
00021 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00022 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00023 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00024 // PURPOSE.  See the GNU General Public License for more details.       //
00025 //                                                                      //
00026 // You should have received a copy of the GNU General Public License    //
00027 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00028 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00029 // Boston, MA 02111-1307 USA.                                           //
00030 // //////////////////////////////////////////////////////////////////// //
00031 //
00032 // Primary maintainer for this file: Christian Siagian <siagian@usc.edu>
00033 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Beobot/TopologicalMap.C $
00034 // $Id:TopologicalMap.C 6891 2006-05-25 06:46:56Z siagian $
00035 //
00036 
00037 // ######################################################################
00038 /*! Topological map for localization                                    */
00039 
00040 #include "Beobot/TopologicalMap.H"
00041 #include "Raster/Raster.H"
00042 #include "Image/DrawOps.H"      // for drawing
00043 #include "Image/MathOps.H"      // for minmax
00044 
00045 #include <cstdio>
00046 
00047 // ######################################################################
00048 TopologicalMap::TopologicalMap()
00049   : Map(),
00050     itsGraph(new Graph())
00051 {
00052 }
00053 
00054 // ######################################################################
00055 TopologicalMap::TopologicalMap(std::string fileName)
00056   : Map(),
00057     itsGraph(new Graph())
00058 {
00059   read(fileName);
00060 }
00061 
00062 // ######################################################################
00063 TopologicalMap::~TopologicalMap()
00064 {
00065 }
00066 
00067 // ######################################################################
00068 bool TopologicalMap::read(std::string fileName)
00069 {
00070   FILE *fp;  char inLine[200]; char comment[200];
00071 
00072   LINFO("file name: %s", fileName.c_str());
00073   if((fp = fopen(fileName.c_str(),"rb")) == NULL)
00074     {
00075       LINFO("not found");
00076       itsMapWidth = 0; itsMapHeight = 0;
00077       LINFO("map size: 0x0");
00078 
00079       itsMapScale = 1.0;
00080       LINFO("map scale: 1.0");
00081 
00082       itsSegmentList.resize(0);
00083       itsSegmentEdgeIndexList.resize(0);
00084       itsSegmentLength.resize(0);
00085       LINFO("segment size: %d", 0);
00086 
00087       return false;
00088     }
00089 
00090   // file creation info:
00091   //  user id, creation date, general map description
00092 
00093   // off-limit areas
00094   //   # of areas
00095   //   description of area (how to characterize the dimensions)
00096 
00097   // named places/position
00098   // need distance relationship wrt to area
00099   //   # of points, description, x,y,theta
00100   //   # of edges to connect the places, with expected distances
00101 
00102   // size of area covered
00103   if (fgets(inLine, 200, fp) == NULL) LFATAL("fgets failed");
00104   int w, h; sscanf(inLine, "%s %d %d", comment, &w, &h);
00105   itsMapWidth = w; itsMapHeight = h;
00106   LINFO("map size: %d %d", itsMapWidth, itsMapHeight);
00107 
00108   // size of area covered
00109   if (fgets(inLine, 200, fp) == NULL) LFATAL("fgets failed");
00110   float scale; sscanf(inLine, "%s %f", comment, &scale);
00111   itsMapScale = scale;
00112   LINFO("map scale: %f", itsMapScale);
00113 
00114   // segment size
00115   if (fgets(inLine, 200, fp) == NULL) LFATAL("fgets failed");
00116   int ssize;  sscanf(inLine, "%s %d", comment, &ssize);
00117   itsSegmentList.resize(ssize);
00118   itsSegmentEdgeIndexList.resize(ssize);
00119   itsSegmentLength.resize(ssize);
00120   LINFO("segment Num: %d", ssize);
00121 
00122   // skip the nodes section divider
00123   if (fgets(inLine, 200, fp) == NULL) LFATAL("fgets failed");
00124 
00125   // get the node coordinates
00126   uint ncount = 0;
00127   while(fgets(inLine, 200, fp) != NULL && inLine[0] != '=')
00128   {
00129     int x, y; sscanf(inLine, "%d %d", &x, &y);
00130     LINFO("node[%d]: (%d,%d)", itsGraph->getNumNode(), x, y);
00131 
00132     rutz::shared_ptr<Node> node(new Node(ncount, Point2D<int>(x,y)));
00133     itsGraph->addNode(node);
00134     ncount++;
00135   }
00136 
00137   // get the edge information
00138   while((fgets(inLine, 200, fp) != NULL) && (inLine[0] != '='))
00139   {
00140     int s, e; sscanf(inLine, "%d %d", &s, &e);
00141     Point2D<int> scoor = itsGraph->getNode(s)->getCoordinate();
00142     Point2D<int> ecoor = itsGraph->getNode(e)->getCoordinate();
00143     float dist = scoor.distance(ecoor);
00144     LINFO("edge(%d,%d): %f", s, e, dist);
00145 
00146     rutz::shared_ptr<Edge>
00147       edge(new Edge(itsGraph->getNode(s),
00148                     itsGraph->getNode(e), dist));
00149     itsGraph->addEdge(edge);
00150   }
00151 
00152   // get the segment information
00153   uint scount = 0;
00154   while(fgets(inLine, 200, fp) != NULL)
00155   {
00156     std::string line(inLine);
00157     while(line.length() > 0)
00158       {
00159         uint cedge;
00160         std::string::size_type spos = line.find_first_of(' ');
00161         if(spos != std::string::npos)
00162           {
00163             cedge = uint(atoi(line.substr(0, spos).c_str()));
00164             line = line.substr(spos+1);
00165           }
00166         else
00167           {
00168             cedge = uint(atoi(line.c_str()));
00169             line = std::string("");
00170           }
00171         LINFO("seg[%d][%"ZU"]:%d",
00172               scount, itsSegmentList[scount].size(), cedge);
00173         itsSegmentList[scount].push_back(itsGraph->getEdge(cedge));
00174         itsSegmentEdgeIndexList[scount].push_back(cedge);
00175       }
00176     scount++;
00177   }
00178 
00179   fclose(fp);
00180 
00181   // compute all the shortcuts for shortest-distance related operations
00182   // node-to-node at graph level
00183   itsGraph->computeAdjecencyList();
00184   itsGraph->computeDistances();
00185 
00186   // node-to-segment, and segment-to-segment at this level
00187   computeDistances();
00188 
00189   // segment length at this level
00190   setSegmentLength();
00191 
00192   return true;
00193 }
00194 
00195 // ######################################################################
00196 void TopologicalMap::computeDistances()
00197 {
00198   // create segment number distance matrix
00199   uint nnode = itsGraph->getNumNode();
00200   uint nsegment = getSegmentNum();
00201   itsNodeSegmentDistanceMatrix.resize(nnode, nsegment);
00202 
00203   // for each node
00204   for(uint i = 0; i < nnode; i++)
00205     {
00206       // for each segment
00207       for(uint j = 0; j < nsegment; j++)
00208         {
00209           // find the closest distance
00210           float mindist;
00211           uint nsedge = itsSegmentList[j].size();
00212           if(nsedge > 0)
00213             {
00214               uint snode = itsSegmentList[j][0]->getSourceNode()->getIndex();
00215               mindist = itsGraph->getUndirectedDistance(i,snode);
00216             }
00217           else mindist = -1.0F;
00218 
00219           for(uint k = 0; k < nsedge; k++)
00220             {
00221               uint dnode = itsSegmentList[j][k]->getDestNode()->getIndex();
00222               float cdist = itsGraph->getUndirectedDistance(i, dnode);
00223               if(mindist > cdist) mindist = cdist;
00224             }
00225 
00226           LDEBUG("itsSegmentList[%d][%d]: %f",i,j, mindist);
00227           itsNodeSegmentDistanceMatrix.setVal(i, j, mindist);
00228         }
00229     }
00230 }
00231 
00232 // ######################################################################
00233 bool TopologicalMap::write(std::string fileName)
00234 {
00235   LFATAL("NOT YET implemented");
00236   return true;
00237 }
00238 
00239 // ######################################################################
00240 Image<PixRGB<byte> > TopologicalMap::getMapImage(uint w, uint h)
00241 {
00242   float scale = std::min(w/itsMapWidth, h/itsMapHeight);
00243 
00244   Image<PixRGB<byte> >
00245     res(int(itsMapWidth * scale), int(itsMapHeight * scale), ZEROS);
00246 
00247   // draw the edges first
00248   for(uint i = 0; i < itsGraph->getNumEdge(); i++)
00249     {
00250       Point2D<int> sp = itsGraph->getEdge(i)->getSourceNode()->getCoordinate();
00251       Point2D<int> ep = itsGraph->getEdge(i)->getDestNode()->getCoordinate();
00252       drawArrow(res,
00253                 Point2D<int>(int(sp.i * scale), int(sp.j * scale)),
00254                 Point2D<int>(int(ep.i * scale), int(ep.j * scale)),
00255                 PixRGB<byte>(127,127,127));
00256     }
00257 
00258   // draw the nodes
00259   for(uint i = 0; i < itsGraph->getNumNode(); i++)
00260     {
00261       Point2D<int> p = itsGraph->getNode(i)->getCoordinate();
00262       Point2D<int> pd(int(p.i * scale), int(p.j * scale));
00263       drawDisk(res, pd, 2, PixRGB<byte>(255,255,255));
00264     }
00265 
00266   drawRect(res,res.getBounds(),PixRGB<byte>(255,255,255));
00267 
00268   return res;
00269 }
00270 
00271 // ######################################################################
00272 float TopologicalMap::getDistance
00273 (uint asnum, float altrav, uint bsnum, float bltrav)
00274 {
00275   ASSERT(asnum < itsSegmentList.size() && altrav <= 1.0F &&
00276          bsnum < itsSegmentList.size() && bltrav <= 1.0F    );
00277 
00278   // get the edges that the two points belongs to
00279   // and the distances to start and end nodes
00280   float dsa, dea, dsb, deb;
00281   rutz::shared_ptr<Edge> a = getEdge(asnum, altrav, dsa, dea);
00282   rutz::shared_ptr<Edge> b = getEdge(bsnum, bltrav, dsb, deb);
00283 
00284   // get the end node indexes
00285   uint sa = a->getSourceNode()->getIndex();
00286   uint ea = a->getDestNode()->getIndex();
00287   uint sb = b->getSourceNode()->getIndex();
00288   uint eb = b->getDestNode()->getIndex();
00289   LDEBUG("%d %d  -- %d %d", sa, ea, sb, eb);
00290 
00291   // same edge situation
00292   // in case we have a directed graph
00293   float dist = 0.0F;
00294   if(sa == sb && ea == eb) dist = fabs(dsa - dsb);
00295   else if(sa == eb && sb == ea) dist = fabs(dsa + dsb - a->getCost());
00296   else
00297     {
00298       // get the shortest distance in
00299       float dist1 = dsa + itsGraph->getUndirectedDistance(sa, sb) + dsb;
00300       float dist2 = dsa + itsGraph->getUndirectedDistance(sa, eb) + deb;
00301       float dist3 = dea + itsGraph->getUndirectedDistance(ea, sb) + dsb;
00302       float dist4 = dea + itsGraph->getUndirectedDistance(ea, eb) + deb;
00303       LDEBUG("%f %f %f %f", dist1,dist2,dist3,dist4);
00304       dist = std::min(std::min(dist1, dist2), std::min(dist3, dist4));
00305     }
00306   LDEBUG("-> %f",dist);
00307   return dist;
00308 }
00309 
00310 // ######################################################################
00311 float TopologicalMap::getPath
00312 (uint asnum, float altrav, uint bsnum, float bltrav,
00313  std::vector<int> &moves)
00314 {
00315   // NOTE: assuming graph is undirected
00316   //       if want directed get path
00317   //       has to find eindexa and b for the opposite direction
00318   //       i.e. edge(ea,sa) and edge(eb,sb)
00319 
00320   ASSERT(asnum < itsSegmentList.size() && altrav <= 1.0F &&
00321          bsnum < itsSegmentList.size() && bltrav <= 1.0F    );
00322   moves.clear();
00323 
00324   // get the edges that the two points belongs to
00325   // and the distances to start and end nodes
00326   float dsa, dea, dsb, deb; uint eindexa, eindexb;
00327   rutz::shared_ptr<Edge> a = getEdge(asnum, altrav, dsa, dea, eindexa);
00328   rutz::shared_ptr<Edge> b = getEdge(bsnum, bltrav, dsb, deb, eindexb);
00329 
00330   // get the end node indexes
00331   uint sa = a->getSourceNode()->getIndex();
00332   uint ea = a->getDestNode()->getIndex();
00333   uint sb = b->getSourceNode()->getIndex();
00334   uint eb = b->getDestNode()->getIndex();
00335   LDEBUG("%d %d  -- %d %d", sa, ea, sb, eb);
00336 
00337   // same edge situation with same start and end
00338   float dist = 0.0F;
00339   if(sa == sb && ea == eb)
00340     {
00341       dist = fabs(dsa - dsb);
00342       if(dsb > dsa) moves.push_back(TOPOMAP_MOVE_FORWARD);
00343       else
00344         {
00345           moves.push_back(TOPOMAP_TURN_AROUND);
00346           moves.push_back(TOPOMAP_MOVE_FORWARD);
00347         }
00348     }
00349   // same edge situation with flipped start and end
00350   else if(sa == eb && sb == ea)
00351     {
00352       dist = fabs(dsa + dsb - a->getCost());
00353       if(deb < dsa) moves.push_back(TOPOMAP_MOVE_FORWARD);
00354       else
00355         {
00356           moves.push_back(TOPOMAP_TURN_AROUND);
00357           moves.push_back(TOPOMAP_MOVE_FORWARD);
00358         }
00359     }
00360   // goes through a few other edges
00361   else
00362     {
00363       // get the shortest distance in
00364       std::vector<uint> edges1;
00365       float dist1 = dsa +
00366         itsGraph->getUndirectedDistance(sa, sb, edges1) + dsb;
00367       std::vector<uint> edges2;
00368       float dist2 = dsa +
00369         itsGraph->getUndirectedDistance(sa, eb, edges2) + deb;
00370       std::vector<uint> edges3;
00371       float dist3 = dea +
00372         itsGraph->getUndirectedDistance(ea, sb, edges3) + dsb;
00373       std::vector<uint> edges4;
00374       float dist4 = dea +
00375         itsGraph->getUndirectedDistance(ea, eb, edges4) + deb;
00376       dist = std::min(std::min(dist1, dist2), std::min(dist3, dist4));
00377       LDEBUG("%f %f %f %f: min: %f", dist1, dist2, dist3, dist4, dist);
00378 
00379       // get the edges
00380       if(dist1 == dist)
00381         {
00382           LDEBUG("E1: %"ZU, edges1.size());
00383           moves.push_back(TOPOMAP_TURN_AROUND);
00384           moves.push_back(TOPOMAP_MOVE_FORWARD);
00385           for(uint i = 0; i < edges1.size(); i++)
00386             {
00387               moves.push_back(edges1[i]);
00388               LDEBUG("E1: %d", edges1[i]);
00389             }
00390           moves.push_back(eindexb);
00391         }
00392       else if(dist2 == dist)
00393         {
00394           LDEBUG("E2: %"ZU, edges2.size());
00395           moves.push_back(TOPOMAP_TURN_AROUND);
00396           moves.push_back(TOPOMAP_MOVE_FORWARD);
00397           for(uint i = 0; i < edges2.size(); i++)
00398             {
00399               moves.push_back(edges2[i]);
00400               LDEBUG("E2: %d", edges2[i]);
00401             }
00402           moves.push_back(eindexb);
00403 
00404           // needs opposite edge eindexb for directed case
00405         }
00406       else if(dist3 == dist)
00407         {
00408           LDEBUG("E3: %"ZU,edges3.size());
00409           moves.push_back(TOPOMAP_MOVE_FORWARD);
00410           for(uint i = 0; i < edges3.size(); i++)
00411             {
00412               moves.push_back(edges3[i]);
00413               LDEBUG("E3: %d", edges3[i]);
00414             }
00415           moves.push_back(eindexb);
00416         }
00417       else if(dist4 == dist)
00418         {
00419           LDEBUG("E4: %"ZU, edges4.size());
00420           moves.push_back(TOPOMAP_MOVE_FORWARD);
00421           for(uint i = 0; i < edges4.size(); i++)
00422             {
00423               moves.push_back(edges4[i]);
00424               LDEBUG("E4: %d", edges4[i]);
00425             }
00426           moves.push_back(eindexb);
00427 
00428           // needs opposite edge eindexb for directed case
00429         }
00430 
00431       for(uint i = 0; i < moves.size(); i++)
00432         LDEBUG("[%d]: %d", i, moves[i]);
00433     }
00434   return dist;
00435 }
00436 
00437 
00438 // ######################################################################
00439 rutz::shared_ptr<Edge> TopologicalMap::getEdge
00440 (uint cseg, float ltrav, float &sdist, float &edist)
00441 {
00442   uint i;
00443   return getEdge(cseg, ltrav, sdist, edist, i);
00444 }
00445 
00446 // ######################################################################
00447 rutz::shared_ptr<Edge> TopologicalMap::getEdge
00448 (uint cseg, float ltrav, float &sdist, float &edist, uint &eindex)
00449 {
00450   ASSERT((cseg < itsSegmentList.size()) && (ltrav <= 1.0F));
00451 
00452   // using the segment we can get its list of edges
00453   float slen = getSegmentLength(cseg);
00454   float altrav = ltrav * slen;
00455   LDEBUG("%d: %f * %f = altrav: %f", cseg, ltrav, slen, altrav);
00456 
00457   // iterate through the list of edges
00458   float cltrav = 0.0F;
00459   uint nsedge = itsSegmentList[cseg].size();
00460   if(nsedge == 0) return rutz::shared_ptr<Edge>();
00461 
00462   uint j = 0;
00463   while((j < nsedge) && (altrav >= cltrav))
00464     { cltrav += itsSegmentList[cseg][j]->getCost(); j++; }
00465   LDEBUG("nsedge: %d -> j: %d", nsedge, j);
00466 
00467   // get the distance to the next node
00468   edist = cltrav - altrav;
00469   sdist = itsSegmentList[cseg][j-1]->getCost() - edist;
00470   LDEBUG("sdist: %f, edist: %f", sdist, edist);
00471   eindex = itsSegmentEdgeIndexList[cseg][j-1];
00472   return itsSegmentList[cseg][j-1];
00473 }
00474 
00475 // ######################################################################
00476 rutz::shared_ptr<Edge> TopologicalMap::getEdge
00477 (Point2D<int> loc, float &sdist, float &edist)
00478 {
00479   LFATAL("NOT YET implemented");
00480 
00481   // can be used to implement conversion from Point2D<int> to (snum,ltrav)
00482   return rutz::shared_ptr<Edge>();
00483 }
00484 
00485 // ######################################################################
00486 float TopologicalMap::getSegmentDistance
00487 (uint sseg, float ltrav, uint dseg, uint &intIndex)
00488 {
00489   ASSERT(sseg < itsSegmentList.size() &&
00490          dseg < itsSegmentList.size() && ltrav <= 1.0F);
00491   //LINFO(" [%d to %d]", sseg, dseg);
00492 
00493   // if the point is in the same segment
00494   // the closest node is an invalid value
00495   if(sseg == dseg) { intIndex = itsGraph->getNumNode(); return 0.0F; }
00496 
00497   // get the edge of the starting point
00498   float slen = getSegmentLength(sseg);
00499   float altrav = ltrav * slen;
00500   float clen = altrav; uint i = 0;
00501   //LINFO("slen: %f, altrav: %f",slen, altrav);
00502 
00503   while(clen > 0.0F)
00504     {
00505       float len = itsSegmentList[sseg][i]->getCost();
00506       if(len > clen)
00507         {
00508           uint sind = itsSegmentList[sseg][i]->getSourceNode()->getIndex();
00509           uint eind = itsSegmentList[sseg][i]->getDestNode()->getIndex();
00510 
00511           float sdist =
00512             itsNodeSegmentDistanceMatrix[Point2D<int>(sind,dseg)] + clen;
00513           float edist =
00514             itsNodeSegmentDistanceMatrix[Point2D<int>(eind,dseg)] + len - clen;
00515 
00516           //LINFO("2 sind: (%d  %f), eind: (%d %f)", sind, sdist, eind, edist);
00517 
00518           if(sdist > edist)
00519             { intIndex = eind; return edist; }
00520           else
00521             { intIndex = sind; return sdist; }
00522         }
00523       else if(len == clen)
00524         {
00525           uint eind = itsSegmentList[sseg][i]->getDestNode()->getIndex();
00526           float edist = itsNodeSegmentDistanceMatrix[Point2D<int>(eind,dseg)];
00527 
00528           //LINFO("1 eind: (%d %f)", eind, edist);
00529           intIndex = eind;
00530           return edist;
00531         }
00532       else{ clen -= len; i++; }
00533       //LINFO("len: %f, clen: %f", len, clen);
00534     }
00535 
00536   // the point is on the source node of the first edge on the list
00537   uint sind = itsSegmentList[sseg][0]->getSourceNode()->getIndex();
00538   float sdist = itsNodeSegmentDistanceMatrix[Point2D<int>(sind,dseg)];
00539   intIndex = sind;
00540   //LINFO("0 sind: (%d  %f)", sind, sdist);
00541 
00542   return sdist;
00543 }
00544 
00545 // ######################################################################
00546 void TopologicalMap::setSegmentLength()
00547 {
00548   // for each segment
00549   for(uint i = 0; i < itsSegmentList.size(); i++)
00550     {
00551       // add up the segment length (cost)
00552       itsSegmentLength[i] = 0.0F;
00553       uint nsedge = itsSegmentList[i].size();
00554       for(uint j = 0; j < nsedge; j++)
00555         itsSegmentLength[i] += itsSegmentList[i][j]->getCost();
00556     }
00557 }
00558 
00559 // ######################################################################
00560 float TopologicalMap::getSegmentLength(uint index)
00561 {
00562   ASSERT(index < itsSegmentLength.size());
00563   return itsSegmentLength[index];
00564 }
00565 
00566 // ######################################################################
00567 float TopologicalMap::getNodeSegmentMaxDistance()
00568 {
00569   float min, max;
00570   getMinMax(itsNodeSegmentDistanceMatrix, min, max);
00571   if(min == -1.0F) return -1.0F;
00572   else return max;
00573 }
00574 
00575 // ######################################################################
00576 void TopologicalMap::getLocationFloat(uint cseg, float ltrav,
00577                                       float &x, float &y)
00578 {
00579   ASSERT((cseg < itsSegmentList.size()) &&
00580          (ltrav <= 1.0F) && (ltrav >= 0.0F));
00581 
00582   // using the segment we can get its list of edges
00583   float slen = getSegmentLength(cseg);
00584   float altrav = ltrav * slen;
00585   LDEBUG("%d: %f * %f = altrav: %f", cseg, ltrav, slen, altrav);
00586 
00587   // iterate through the list of edges
00588   float cltrav = 0.0F;
00589   uint nsedge = itsSegmentList[cseg].size();
00590   if(nsedge == 0) { x = -1.0F; y = -1.0F; return; }
00591   uint j = 0;
00592   while((j < nsedge) && (altrav >= cltrav))
00593     { cltrav += itsSegmentList[cseg][j]->getCost(); j++; }
00594   LDEBUG("nsedge: %d -> j: %d", nsedge, j);
00595 
00596   // get the distance to the next node
00597   float rdist = cltrav - altrav;
00598   float dist = itsSegmentList[cseg][j-1]->getCost();
00599   Point2D<int> sloc =
00600     itsSegmentList[cseg][j-1]->getSourceNode()->getCoordinate();
00601   Point2D<int> eloc =
00602     itsSegmentList[cseg][j-1]->getDestNode()->getCoordinate();
00603   LDEBUG("%f/%f (%d,%d) -> (%d,%d)",
00604         rdist, dist, sloc.i,sloc.j, eloc.i, eloc.j);
00605 
00606   x = eloc.i - rdist/dist*(eloc.i - sloc.i);
00607   y = eloc.j - rdist/dist*(eloc.j - sloc.j);
00608 }
00609 
00610 // ######################################################################
00611 Point2D<int> TopologicalMap::getLocation(uint cseg, float ltrav)
00612 {
00613   float x, y;
00614   getLocationFloat(cseg, ltrav, x, y);
00615   uint iloc = uint(x + 0.5F);
00616   uint jloc = uint(y + 0.5F);
00617   LDEBUG("(%d, %f) -> %d, %d", cseg, ltrav, iloc, jloc);
00618   return Point2D<int>(iloc, jloc);
00619 }
00620 
00621 // ######################################################################
00622 void TopologicalMap::getLocation(Point2D<int>, uint &cseg, float &ltrav)
00623 {
00624   // use get edge (also not yet implemented)
00625   // to implement conversion from Point2D<int> to (snum,ltrav)
00626 
00627   // check if the point is within the map
00628   LFATAL("NOT YET implemented");
00629 }
00630 
00631 // ######################################################################
00632 uint TopologicalMap::getSegmentLocation(Point2D<int> loc)
00633 {
00634   // use get edge (also not yet implemented)
00635   // to implement conversion from Point2D<int> to (snum,ltrav)
00636 
00637   LFATAL("NOT YET implemented");
00638 
00639   return 0;
00640 }
00641 
00642 // ######################################################################
00643 float TopologicalMap::getSegmentLengthTraveled(Point2D<int> loc)
00644 {
00645   // use get edge (also not yet implemented)
00646   // to implement conversion from Point2D<int> to (snum,ltrav)
00647 
00648   LFATAL("NOT YET implemented");
00649   return 0.0;
00650 }
00651 
00652 // ######################################################################
00653 std::vector<rutz::shared_ptr<Node> >
00654 TopologicalMap::getNodesInInterval(uint index, float fltrav, float lltrav)
00655 {
00656   ASSERT(index < itsSegmentLength.size());
00657   ASSERT(fltrav >= 0.0 && fltrav <= 1.0);
00658   ASSERT(lltrav >= 0.0 && lltrav <= 1.0);
00659 
00660   float slen = getSegmentLength(index);
00661   std::vector<rutz::shared_ptr<Node> > res;
00662   float caltrav = 0.0;
00663 
00664   // go through the edges in the segment
00665   uint ssize = itsSegmentList[index].size();
00666   for(uint i = 0; i < ssize; i++)
00667     {
00668       // check if the source node is within the interval
00669       float ltrav = caltrav/slen;
00670       LDEBUG("%d: %f * %f = altrav: %f", index, ltrav, slen, caltrav);
00671 
00672       if(ltrav >= fltrav && ltrav <= lltrav)
00673         res.push_back(itsSegmentList[index][i]->getSourceNode());
00674       else if(ltrav > lltrav) return res;
00675 
00676       caltrav += itsSegmentList[index][i]->getCost();
00677     }
00678 
00679   if (lltrav == 1.0)
00680     res.push_back(itsSegmentList[index][ssize-1]->getDestNode());
00681 
00682   return res;
00683 }
00684 
00685 // ######################################################################
00686 std::vector<std::pair<uint,float> >
00687 TopologicalMap::getNodeLocationsInInterval
00688 (uint index, float fltrav, float lltrav)
00689 {
00690   ASSERT(index < itsSegmentLength.size());
00691   ASSERT(fltrav >= 0.0 && fltrav <= 1.0);
00692   ASSERT(lltrav >= 0.0 && lltrav <= 1.0);
00693 
00694   float slen = getSegmentLength(index);
00695   std::vector<std::pair<uint,float> > res;
00696   float caltrav = 0.0;
00697 
00698   // go through the edges in the segment
00699   uint ssize = itsSegmentList[index].size();
00700   for(uint i = 0; i < ssize; i++)
00701     {
00702       // check if the source node is within the interval
00703       float ltrav = caltrav/slen;
00704       LDEBUG("%d: %f * %f = caltrav: %f", index, ltrav, slen, caltrav);
00705 
00706       if(ltrav >= fltrav && ltrav <= lltrav)
00707         res.push_back(std::pair<uint,float>(index, ltrav));
00708       else if(ltrav > lltrav) return res;
00709 
00710       caltrav += itsSegmentList[index][i]->getCost();
00711     }
00712 
00713   if (lltrav == 1.0F)
00714     res.push_back(std::pair<uint,float>(index, 1.0F));
00715 
00716   return res;
00717 }
00718 
00719 // ######################################################################
00720 float TopologicalMap::getAngle
00721 (rutz::shared_ptr<Edge> e1, rutz::shared_ptr<Edge> e2)
00722 {
00723   return itsGraph->getAngle(e1, e2);
00724 }
00725 
00726 // ######################################################################
00727 /* So things look consistent in everyone's emacs... */
00728 /* Local Variables: */
00729 /* indent-tabs-mode: nil */
00730 /* End: */
Generated on Sun May 8 08:40:15 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3